ferravex_math/
quaternion.rs

1use crate::{Vec3, Vec4, vec3};
2use std::ops::{Add, Div, Mul, MulAssign, Neg, Sub};
3
4/// Intrinsic Euler rotations: `XYZ(a,b,c)` composes `Rx(a) * Ry(b) * Rz(c)`.
5#[derive(Clone, Copy, Debug, PartialEq, Eq)]
6pub enum EulerRot {
7    XYZ,
8    XZY,
9    YXZ,
10    YZX,
11    ZXY,
12    ZYX,
13}
14
15/// Quaternion stored as vector part `(x,y,z)` followed by scalar part `w`.
16#[repr(C)]
17#[derive(Clone, Copy, Debug, PartialEq)]
18pub struct Quat {
19    pub x: f32,
20    pub y: f32,
21    pub z: f32,
22    pub w: f32,
23}
24impl Default for Quat {
25    fn default() -> Self {
26        Self::IDENTITY
27    }
28}
29impl Quat {
30    pub const IDENTITY: Self = Self::from_xyzw(0.0, 0.0, 0.0, 1.0);
31    pub const NAN: Self = Self::from_xyzw(f32::NAN, f32::NAN, f32::NAN, f32::NAN);
32    pub const fn from_xyzw(x: f32, y: f32, z: f32, w: f32) -> Self {
33        Self { x, y, z, w }
34    }
35    pub const fn from_array(v: [f32; 4]) -> Self {
36        Self::from_xyzw(v[0], v[1], v[2], v[3])
37    }
38    pub const fn to_array(self) -> [f32; 4] {
39        [self.x, self.y, self.z, self.w]
40    }
41    pub const fn from_vec4(v: Vec4) -> Self {
42        Self::from_xyzw(v.x, v.y, v.z, v.w)
43    }
44    pub const fn to_vec4(self) -> Vec4 {
45        Vec4::new(self.x, self.y, self.z, self.w)
46    }
47    pub const fn xyz(self) -> Vec3 {
48        vec3(self.x, self.y, self.z)
49    }
50    /// Finite nonzero axes are normalized. A zero axis returns NaNs.
51    pub fn from_axis_angle(axis: Vec3, angle: f32) -> Self {
52        let unit = axis.normalize();
53        let (s, c) = (angle * 0.5).sin_cos();
54        Self::from_xyzw(unit.x * s, unit.y * s, unit.z * s, c)
55    }
56    pub fn from_rotation_x(angle: f32) -> Self {
57        let (s, c) = (angle * 0.5).sin_cos();
58        Self::from_xyzw(s, 0.0, 0.0, c)
59    }
60    pub fn from_rotation_y(angle: f32) -> Self {
61        let (s, c) = (angle * 0.5).sin_cos();
62        Self::from_xyzw(0.0, s, 0.0, c)
63    }
64    pub fn from_rotation_z(angle: f32) -> Self {
65        let (s, c) = (angle * 0.5).sin_cos();
66        Self::from_xyzw(0.0, 0.0, s, c)
67    }
68    pub fn from_euler(order: EulerRot, a: f32, b: f32, c: f32) -> Self {
69        use EulerRot::*;
70        match order {
71            XYZ => Self::from_rotation_x(a) * Self::from_rotation_y(b) * Self::from_rotation_z(c),
72            XZY => Self::from_rotation_x(a) * Self::from_rotation_z(b) * Self::from_rotation_y(c),
73            YXZ => Self::from_rotation_y(a) * Self::from_rotation_x(b) * Self::from_rotation_z(c),
74            YZX => Self::from_rotation_y(a) * Self::from_rotation_z(b) * Self::from_rotation_x(c),
75            ZXY => Self::from_rotation_z(a) * Self::from_rotation_x(b) * Self::from_rotation_y(c),
76            ZYX => Self::from_rotation_z(a) * Self::from_rotation_y(b) * Self::from_rotation_x(c),
77        }
78    }
79    pub fn from_scaled_axis(axis: Vec3) -> Self {
80        if axis == Vec3::ZERO {
81            Self::IDENTITY
82        } else {
83            Self::from_axis_angle(axis, axis.length())
84        }
85    }
86    /// Shortest rotation between two finite nonzero directions.
87    pub fn from_rotation_arc(from: Vec3, to: Vec3) -> Self {
88        let a = from.normalize();
89        let b = to.normalize();
90        let cosine = a.dot(b).clamp(-1.0, 1.0);
91        if cosine > 1.0 - 1.0e-6 {
92            return Self::IDENTITY;
93        }
94        if cosine < -1.0 + 1.0e-6 {
95            return Self::from_axis_angle(a.any_orthonormal_vector(), std::f32::consts::PI);
96        }
97        let cross = a.cross(b);
98        Self::from_xyzw(cross.x, cross.y, cross.z, 1.0 + cosine).normalize()
99    }
100    pub fn to_axis_angle(self) -> (Vec3, f32) {
101        let q = self.normalize();
102        let sine = q.xyz().length();
103        if sine < 1.0e-7 {
104            (Vec3::X, 0.0)
105        } else {
106            (q.xyz() / sine, 2.0 * sine.atan2(q.w))
107        }
108    }
109    pub fn to_scaled_axis(self) -> Vec3 {
110        let (axis, angle) = self.to_axis_angle();
111        axis * angle
112    }
113    pub fn dot(self, rhs: Self) -> f32 {
114        self.to_vec4().dot(rhs.to_vec4())
115    }
116    pub fn length_squared(self) -> f32 {
117        self.dot(self)
118    }
119    pub fn length(self) -> f32 {
120        self.to_vec4().length()
121    }
122    pub fn is_finite(self) -> bool {
123        self.to_vec4().is_finite()
124    }
125    pub fn is_nan(self) -> bool {
126        self.to_vec4().is_nan()
127    }
128    pub fn is_normalized(self) -> bool {
129        self.to_vec4().is_normalized()
130    }
131    pub fn try_normalize(self) -> Option<Self> {
132        self.to_vec4().try_normalize().map(Self::from_vec4)
133    }
134    pub fn normalize(self) -> Self {
135        self.try_normalize().unwrap_or(Self::NAN)
136    }
137    pub const fn conjugate(self) -> Self {
138        Self::from_xyzw(-self.x, -self.y, -self.z, self.w)
139    }
140    /// Algebraic inverse, also correct for finite non-unit quaternions.
141    pub fn try_inverse(self) -> Option<Self> {
142        let a = self.to_array();
143        let norm = a.iter().map(|x| f64::from(*x).powi(2)).sum::<f64>();
144        if norm == 0.0 || !norm.is_finite() {
145            return None;
146        }
147        let result = Self::from_array(
148            self.conjugate()
149                .to_array()
150                .map(|x| (f64::from(x) / norm) as f32),
151        );
152        result.is_finite().then_some(result)
153    }
154    pub fn inverse(self) -> Self {
155        self.try_inverse().unwrap_or(Self::NAN)
156    }
157    /// Applies a unit quaternion rotation. Non-unit quaternions must be normalized first.
158    pub fn mul_vec3(self, v: Vec3) -> Vec3 {
159        let t = 2.0 * self.xyz().cross(v);
160        v + self.w * t + self.xyz().cross(t)
161    }
162    pub fn mul_quat(self, rhs: Self) -> Self {
163        self * rhs
164    }
165    pub fn lerp(self, rhs: Self, t: f32) -> Self {
166        let rhs = if self.dot(rhs) < 0.0 { -rhs } else { rhs };
167        (self * (1.0 - t) + rhs * t).normalize()
168    }
169    /// Spherical interpolation follows the shortest rotation and normalizes endpoints.
170    pub fn slerp(self, rhs: Self, t: f32) -> Self {
171        let a = self.normalize();
172        let mut b = rhs.normalize();
173        let mut cosine = a.dot(b);
174        if cosine < 0.0 {
175            b = -b;
176            cosine = -cosine;
177        }
178        if cosine > 0.9995 {
179            return a.lerp(b, t);
180        }
181        let angle = cosine.clamp(-1.0, 1.0).acos();
182        let divisor = angle.sin();
183        (a * ((1.0 - t) * angle).sin() / divisor + b * (t * angle).sin() / divisor).normalize()
184    }
185    pub fn angle_between(self, rhs: Self) -> f32 {
186        2.0 * self
187            .normalize()
188            .dot(rhs.normalize())
189            .abs()
190            .clamp(0.0, 1.0)
191            .acos()
192    }
193    pub fn abs_diff_eq(self, rhs: Self, tolerance: f32) -> bool {
194        self.to_vec4().abs_diff_eq(rhs.to_vec4(), tolerance)
195    }
196    pub fn rotate_towards(self, rhs: Self, max_angle: f32) -> Self {
197        assert!(max_angle.is_finite() && max_angle >= 0.0);
198        let angle = self.angle_between(rhs);
199        if angle <= max_angle {
200            rhs.normalize()
201        } else {
202            self.slerp(rhs, max_angle / angle)
203        }
204    }
205}
206impl From<[f32; 4]> for Quat {
207    fn from(v: [f32; 4]) -> Self {
208        Self::from_array(v)
209    }
210}
211impl From<Quat> for [f32; 4] {
212    fn from(v: Quat) -> Self {
213        v.to_array()
214    }
215}
216impl From<Quat> for Vec4 {
217    fn from(v: Quat) -> Self {
218        v.to_vec4()
219    }
220}
221impl Neg for Quat {
222    type Output = Self;
223    fn neg(self) -> Self {
224        Self::from_vec4(-self.to_vec4())
225    }
226}
227impl Add for Quat {
228    type Output = Self;
229    fn add(self, rhs: Self) -> Self {
230        Self::from_vec4(self.to_vec4() + rhs.to_vec4())
231    }
232}
233impl Sub for Quat {
234    type Output = Self;
235    fn sub(self, rhs: Self) -> Self {
236        Self::from_vec4(self.to_vec4() - rhs.to_vec4())
237    }
238}
239impl Mul<f32> for Quat {
240    type Output = Self;
241    fn mul(self, rhs: f32) -> Self {
242        Self::from_vec4(self.to_vec4() * rhs)
243    }
244}
245impl Mul<Quat> for f32 {
246    type Output = Quat;
247    fn mul(self, rhs: Quat) -> Quat {
248        rhs * self
249    }
250}
251impl Div<f32> for Quat {
252    type Output = Self;
253    fn div(self, rhs: f32) -> Self {
254        Self::from_vec4(self.to_vec4() / rhs)
255    }
256}
257impl Mul for Quat {
258    type Output = Self;
259    fn mul(self, rhs: Self) -> Self {
260        let vector = self.w * rhs.xyz() + rhs.w * self.xyz() + self.xyz().cross(rhs.xyz());
261        Self::from_xyzw(
262            vector.x,
263            vector.y,
264            vector.z,
265            self.w * rhs.w - self.xyz().dot(rhs.xyz()),
266        )
267    }
268}
269impl Mul<Vec3> for Quat {
270    type Output = Vec3;
271    fn mul(self, rhs: Vec3) -> Vec3 {
272        self.mul_vec3(rhs)
273    }
274}
275impl MulAssign for Quat {
276    fn mul_assign(&mut self, rhs: Self) {
277        *self = *self * rhs;
278    }
279}
ferravex_math/quaternion.rs

ferravex_math/
quaternion.rs
