khora_core/math/
quaternion.rs

1// Copyright 2025 eraflo
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7//     http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14
15//! Provides a Quaternion type for representing 3D rotations.
16
17use bincode::{Decode, Encode};
18use serde::{Deserialize, Serialize};
19
20use super::{Mat4, Vec3, EPSILON};
21use std::ops::{Add, Mul, MulAssign, Neg, Sub};
22
23/// Represents a quaternion for efficient 3D rotations.
24///
25/// Quaternions are a four-dimensional complex number system that can represent
26/// rotations in 3D space. They are generally more efficient and numerically stable
27/// than rotation matrices, avoiding issues like gimbal lock.
28///
29/// A quaternion is stored as `(x, y, z, w)`, where `[x, y, z]` is the "vector" part
30/// and `w` is the "scalar" part. For representing rotations, it should be a "unit
31/// quaternion" where `x² + y² + z² + w² = 1`.
32///
33/// # Examples
34///
35/// ```rust
36/// use khora_core::math::{Quaternion, Vec3};
37/// use std::f32::consts::FRAC_PI_2;
38///
39/// // A quarter-turn about the Y axis maps +Z onto +X.
40/// let yaw = Quaternion::from_axis_angle(Vec3::Y, FRAC_PI_2);
41/// let rotated = yaw * Vec3::Z;
42/// assert!((rotated - Vec3::X).length() < 1e-5);
43///
44/// // Composing rotations is quaternion multiplication; the identity is a no-op.
45/// let combined = yaw * Quaternion::IDENTITY;
46/// assert!((combined * Vec3::Z - Vec3::X).length() < 1e-5);
47///
48/// // `slerp` blends along the shortest arc; halfway is a 45° turn.
49/// let half = Quaternion::slerp(Quaternion::IDENTITY, yaw, 0.5);
50/// let expected = Quaternion::from_axis_angle(Vec3::Y, FRAC_PI_2 / 2.0);
51/// assert!((half * Vec3::Z - expected * Vec3::Z).length() < 1e-5);
52/// ```
53#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize, Encode, Decode)]
54#[repr(C)]
55pub struct Quaternion {
56    /// The x component of the vector part.
57    pub x: f32,
58    /// The y component of the vector part.
59    pub y: f32,
60    /// The z component of the vector part.
61    pub z: f32,
62    /// The scalar (real) part.
63    pub w: f32,
64}
65
66/// Alias for [`Quaternion`] for brevity.
67pub type Quat = Quaternion;
68
69impl Quaternion {
70    /// The identity quaternion, representing no rotation.
71    pub const IDENTITY: Quaternion = Quaternion {
72        x: 0.0,
73        y: 0.0,
74        z: 0.0,
75        w: 1.0,
76    };
77
78    /// Creates a new quaternion from its raw components.
79    ///
80    /// Note: This does not guarantee a unit quaternion. For creating rotations,
81    /// prefer using `from_axis_angle` or other rotation-specific constructors.
82    #[inline]
83    pub fn new(x: f32, y: f32, z: f32, w: f32) -> Self {
84        Self { x, y, z, w }
85    }
86
87    /// Creates a quaternion representing a rotation around a given axis by a given angle.
88    ///
89    /// # Arguments
90    ///
91    /// * `axis`: The axis of rotation. It is recommended to pass a normalized vector.
92    /// * `angle_radians`: The angle of rotation in radians.
93    #[inline]
94    pub fn from_axis_angle(axis: Vec3, angle_radians: f32) -> Self {
95        let normalized_axis = axis.normalize();
96        let half_angle = angle_radians * 0.5;
97        let s = half_angle.sin();
98        let c = half_angle.cos();
99        Self {
100            x: normalized_axis.x * s,
101            y: normalized_axis.y * s,
102            z: normalized_axis.z * s,
103            w: c,
104        }
105    }
106
107    /// Creates a quaternion from a 4x4 rotation matrix.
108    ///
109    /// This method only considers the upper 3x3 part of the matrix for the conversion.
110    #[inline]
111    pub fn from_rotation_matrix(m: &Mat4) -> Self {
112        let m00 = m.cols[0].x;
113        let m10 = m.cols[0].y;
114        let m20 = m.cols[0].z;
115        let m01 = m.cols[1].x;
116        let m11 = m.cols[1].y;
117        let m21 = m.cols[1].z;
118        let m02 = m.cols[2].x;
119        let m12 = m.cols[2].y;
120        let m22 = m.cols[2].z;
121
122        // Algorithm from http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
123        let trace = m00 + m11 + m22;
124        let mut q = Self::IDENTITY;
125
126        if trace > 0.0 {
127            let s = 2.0 * (trace + 1.0).sqrt();
128            q.w = 0.25 * s;
129            q.x = (m21 - m12) / s;
130            q.y = (m02 - m20) / s;
131            q.z = (m10 - m01) / s;
132        } else if m00 > m11 && m00 > m22 {
133            let s = 2.0 * (1.0 + m00 - m11 - m22).sqrt();
134            q.w = (m21 - m12) / s;
135            q.x = 0.25 * s;
136            q.y = (m01 + m10) / s;
137            q.z = (m02 + m20) / s;
138        } else if m11 > m22 {
139            let s = 2.0 * (1.0 + m11 - m00 - m22).sqrt();
140            q.w = (m02 - m20) / s;
141            q.x = (m01 + m10) / s;
142            q.y = 0.25 * s;
143            q.z = (m12 + m21) / s;
144        } else {
145            let s = 2.0 * (1.0 + m22 - m00 - m11).sqrt();
146            q.w = (m10 - m01) / s;
147            q.x = (m02 + m20) / s;
148            q.y = (m12 + m21) / s;
149            q.z = 0.25 * s;
150        }
151        q.normalize()
152    }
153
154    /// Calculates the squared length (magnitude) of the quaternion.
155    #[inline]
156    pub fn magnitude_squared(&self) -> f32 {
157        self.x * self.x + self.y * self.y + self.z * self.z + self.w * self.w
158    }
159
160    /// Calculates the length (magnitude) of the quaternion.
161    #[inline]
162    pub fn magnitude(&self) -> f32 {
163        self.magnitude_squared().sqrt()
164    }
165
166    /// Returns a normalized version of the quaternion with a length of 1.
167    /// If the quaternion has a near-zero magnitude, it returns the identity quaternion.
168    pub fn normalize(&self) -> Self {
169        let mag_sqrt = self.magnitude_squared();
170        if mag_sqrt > EPSILON {
171            let inv_mag = 1.0 / mag_sqrt.sqrt();
172            Self {
173                x: self.x * inv_mag,
174                y: self.y * inv_mag,
175                z: self.z * inv_mag,
176                w: self.w * inv_mag,
177            }
178        } else {
179            Self::IDENTITY
180        }
181    }
182
183    /// Computes the conjugate of the quaternion, which negates the vector part.
184    #[inline]
185    pub fn conjugate(&self) -> Self {
186        Self {
187            x: -self.x,
188            y: -self.y,
189            z: -self.z,
190            w: self.w,
191        }
192    }
193
194    /// Computes the inverse of the quaternion.
195    /// For a unit quaternion, the inverse is equal to its conjugate.
196    #[inline]
197    pub fn inverse(&self) -> Self {
198        let mag_squared = self.magnitude_squared();
199        if mag_squared > EPSILON {
200            self.conjugate() * (1.0 / mag_squared)
201        } else {
202            Self::IDENTITY
203        }
204    }
205
206    /// Computes the dot product of two quaternions.
207    #[inline]
208    pub fn dot(&self, other: Self) -> f32 {
209        self.x * other.x + self.y * other.y + self.z * other.z + self.w * other.w
210    }
211
212    /// Rotates a 3D vector by this quaternion.
213    pub fn rotate_vec3(&self, v: Vec3) -> Vec3 {
214        let u = Vec3::new(self.x, self.y, self.z);
215        let s: f32 = self.w;
216        2.0 * u.dot(v) * u + (s * s - u.dot(u)) * v + 2.0 * s * u.cross(v)
217    }
218
219    /// Performs a Spherical Linear Interpolation (Slerp) between two quaternions.
220    ///
221    /// Slerp provides a smooth, constant-speed interpolation between two rotations,
222    /// following the shortest path on the surface of a 4D sphere.
223    ///
224    /// *   `t` - The interpolation factor, clamped to the `[0.0, 1.0]` range.
225    pub fn slerp(start: Self, end: Self, t: f32) -> Self {
226        let t = t.clamp(0.0, 1.0);
227        let mut cos_theta = start.dot(end);
228        let mut end_adjusted = end;
229
230        // If the dot product is negative, the quaternions are more than 90 degrees apart.
231        // To ensure the shortest path, negate one quaternion.
232        // This is equivalent to using the conjugate of the quaternion.
233        if cos_theta < 0.0 {
234            cos_theta = -cos_theta;
235            end_adjusted = -end;
236        }
237
238        if cos_theta > 1.0 - EPSILON {
239            // Linear Interpolation: (1-t)*start + t*end_adjusted
240            // Normalize the result to avoid drift due to floating point errors.
241            let result = (start * (1.0 - t)) + (end_adjusted * t);
242            result.normalize()
243        } else {
244            let angle = cos_theta.acos();
245            let sin_theta_inv = 1.0 / angle.sin();
246            let scale_start = ((1.0 - t) * angle).sin() * sin_theta_inv;
247            let scale_end = (t * angle).sin() * sin_theta_inv;
248            (start * scale_start) + (end_adjusted * scale_end)
249        }
250    }
251
252    /// Converts this quaternion to Euler angles in XYZ (pitch, yaw, roll) order.
253    ///
254    /// Returns `(x, y, z)` angles in radians.
255    /// - `x` = pitch (rotation around X axis)
256    /// - `y` = yaw   (rotation around Y axis)
257    /// - `z` = roll  (rotation around Z axis)
258    pub fn to_euler_xyz(&self) -> (f32, f32, f32) {
259        // Roll (X-axis rotation)
260        let sinr_cosp = 2.0 * (self.w * self.x + self.y * self.z);
261        let cosr_cosp = 1.0 - 2.0 * (self.x * self.x + self.y * self.y);
262        let x = sinr_cosp.atan2(cosr_cosp);
263
264        // Pitch (Y-axis rotation)
265        let sinp = 2.0 * (self.w * self.y - self.z * self.x);
266        let y = if sinp.abs() >= 1.0 {
267            std::f32::consts::FRAC_PI_2.copysign(sinp)
268        } else {
269            sinp.asin()
270        };
271
272        // Yaw (Z-axis rotation)
273        let siny_cosp = 2.0 * (self.w * self.z + self.x * self.y);
274        let cosy_cosp = 1.0 - 2.0 * (self.y * self.y + self.z * self.z);
275        let z = siny_cosp.atan2(cosy_cosp);
276
277        (x, y, z)
278    }
279
280    /// Creates a quaternion from Euler angles in XYZ order.
281    ///
282    /// # Arguments
283    /// - `x` — pitch in radians (rotation around X axis)
284    /// - `y` — yaw in radians   (rotation around Y axis)
285    /// - `z` — roll in radians  (rotation around Z axis)
286    pub fn from_euler_xyz(x: f32, y: f32, z: f32) -> Self {
287        let (sx, cx) = (x * 0.5).sin_cos();
288        let (sy, cy) = (y * 0.5).sin_cos();
289        let (sz, cz) = (z * 0.5).sin_cos();
290
291        Self {
292            x: sx * cy * cz + cx * sy * sz,
293            y: cx * sy * cz - sx * cy * sz,
294            z: cx * cy * sz + sx * sy * cz,
295            w: cx * cy * cz - sx * sy * sz,
296        }
297    }
298}
299
300// --- Operator Overloads ---
301
302impl Default for Quaternion {
303    /// Returns the identity quaternion, representing no rotation.
304    #[inline]
305    fn default() -> Self {
306        Self::IDENTITY
307    }
308}
309
310impl Mul<Quaternion> for Quaternion {
311    type Output = Self;
312    /// Combines two rotations using the Hamilton product.
313    /// Note that quaternion multiplication is not commutative.
314    #[inline]
315    fn mul(self, rhs: Self) -> Self::Output {
316        Self {
317            x: self.w * rhs.x + self.x * rhs.w + self.y * rhs.z - self.z * rhs.y,
318            y: self.w * rhs.y - self.x * rhs.z + self.y * rhs.w + self.z * rhs.x,
319            z: self.w * rhs.z + self.x * rhs.y - self.y * rhs.x + self.z * rhs.w,
320            w: self.w * rhs.w - self.x * rhs.x - self.y * rhs.y - self.z * rhs.z,
321        }
322    }
323}
324
325impl MulAssign<Quaternion> for Quaternion {
326    /// Combines this rotation with another.
327    #[inline]
328    fn mul_assign(&mut self, rhs: Self) {
329        *self = *self * rhs;
330    }
331}
332
333impl Mul<Vec3> for Quaternion {
334    type Output = Vec3;
335    /// Rotates a `Vec3` by this quaternion.
336    #[inline]
337    fn mul(self, rhs: Vec3) -> Self::Output {
338        self.normalize().rotate_vec3(rhs)
339    }
340}
341
342impl Add<Quaternion> for Quaternion {
343    type Output = Self;
344    /// Adds two quaternions component-wise.
345    /// Note: This is not a standard rotation operation.
346    #[inline]
347    fn add(self, rhs: Self) -> Self::Output {
348        Self {
349            x: self.x + rhs.x,
350            y: self.y + rhs.y,
351            z: self.z + rhs.z,
352            w: self.w + rhs.w,
353        }
354    }
355}
356
357impl Sub<Quaternion> for Quaternion {
358    type Output = Self;
359    /// Subtracts two quaternions component-wise.
360    #[inline]
361    fn sub(self, rhs: Self) -> Self::Output {
362        Self {
363            x: self.x - rhs.x,
364            y: self.y - rhs.y,
365            z: self.z - rhs.z,
366            w: self.w - rhs.w,
367        }
368    }
369}
370
371impl Mul<f32> for Quaternion {
372    type Output = Self;
373    /// Scales all components of the quaternion by a scalar.
374    #[inline]
375    fn mul(self, scalar: f32) -> Self::Output {
376        Self {
377            x: self.x * scalar,
378            y: self.y * scalar,
379            z: self.z * scalar,
380            w: self.w * scalar,
381        }
382    }
383}
384
385impl Neg for Quaternion {
386    type Output = Self;
387    /// Negates all components of the quaternion.
388    #[inline]
389    fn neg(self) -> Self::Output {
390        Self {
391            x: -self.x,
392            y: -self.y,
393            z: -self.z,
394            w: -self.w,
395        }
396    }
397}
398
399#[cfg(test)]
400mod tests {
401    use super::*; // Import Quaternion, EPSILON etc. from parent module
402    use crate::math::vector::Vec4;
403    use approx::assert_relative_eq; // For float comparisons
404
405    fn quat_approx_eq(q1: Quaternion, q2: Quaternion) -> bool {
406        let dot = q1.dot(q2).abs();
407        approx::relative_eq!(dot, 1.0, epsilon = EPSILON * 10.0) // Use abs dot product
408    }
409
410    #[test]
411    fn test_identity_and_default() {
412        let q_ident = Quaternion::IDENTITY;
413        let q_def = Quaternion::default();
414        assert_eq!(q_ident, q_def);
415        assert_relative_eq!(q_ident.x, 0.0);
416        assert_relative_eq!(q_ident.y, 0.0);
417        assert_relative_eq!(q_ident.z, 0.0);
418        assert_relative_eq!(q_ident.w, 1.0);
419        assert_relative_eq!(q_ident.magnitude(), 1.0, epsilon = EPSILON);
420    }
421
422    #[test]
423    fn test_from_axis_angle() {
424        let axis = Vec3::Y;
425        let angle = std::f32::consts::FRAC_PI_2; // 90 degrees
426        let q = Quaternion::from_axis_angle(axis, angle);
427
428        let half_angle = angle * 0.5;
429        let expected_s = half_angle.sin();
430        let expected_c = half_angle.cos();
431
432        assert_relative_eq!(q.x, 0.0 * expected_s, epsilon = EPSILON);
433        assert_relative_eq!(q.y, 1.0 * expected_s, epsilon = EPSILON);
434        assert_relative_eq!(q.z, 0.0 * expected_s, epsilon = EPSILON);
435        assert_relative_eq!(q.w, expected_c, epsilon = EPSILON);
436        assert_relative_eq!(q.magnitude(), 1.0, epsilon = EPSILON);
437    }
438
439    #[test]
440    fn test_from_axis_angle_normalizes_axis() {
441        let axis = Vec3::new(0.0, 5.0, 0.0); // Non-unit axis
442        let angle = std::f32::consts::FRAC_PI_2;
443        let q = Quaternion::from_axis_angle(axis, angle);
444
445        let half_angle = angle * 0.5;
446        let expected_s = half_angle.sin();
447        let expected_c = half_angle.cos();
448
449        assert_relative_eq!(q.x, 0.0 * expected_s, epsilon = EPSILON);
450        assert_relative_eq!(q.y, 1.0 * expected_s, epsilon = EPSILON); // Normalized axis y=1.0
451        assert_relative_eq!(q.z, 0.0 * expected_s, epsilon = EPSILON);
452        assert_relative_eq!(q.w, expected_c, epsilon = EPSILON);
453        assert_relative_eq!(q.magnitude(), 1.0, epsilon = EPSILON);
454    }
455
456    #[test]
457    fn test_from_rotation_matrix_identity() {
458        let m = Mat4::IDENTITY;
459        let q = Quaternion::from_rotation_matrix(&m);
460        assert!(quat_approx_eq(q, Quaternion::IDENTITY));
461    }
462
463    #[test]
464    fn test_from_rotation_matrix_simple_rotations() {
465        let angle = std::f32::consts::FRAC_PI_4; // 45 degrees
466
467        // Rotation X
468        let mx = Mat4::from_rotation_x(angle);
469        let qx_expected = Quaternion::from_axis_angle(Vec3::X, angle);
470        let qx_from_m = Quaternion::from_rotation_matrix(&mx);
471        assert!(quat_approx_eq(qx_from_m, qx_expected));
472
473        // Rotation Y
474        let my = Mat4::from_rotation_y(angle);
475        let qy_expected = Quaternion::from_axis_angle(Vec3::Y, angle);
476        let qy_from_m = Quaternion::from_rotation_matrix(&my);
477        assert!(quat_approx_eq(qy_from_m, qy_expected));
478
479        // Rotation Z
480        let mz = Mat4::from_rotation_z(angle);
481        let qz_expected = Quaternion::from_axis_angle(Vec3::Z, angle);
482        let qz_from_m = Quaternion::from_rotation_matrix(&mz);
483        assert!(quat_approx_eq(qz_from_m, qz_expected));
484    }
485
486    #[test]
487    fn test_matrix_to_quat_and_back() {
488        let axis = Vec3::new(-1.0, 2.5, 0.7).normalize();
489        let angle = 1.85; // Some arbitrary angle
490
491        let q_orig = Quaternion::from_axis_angle(axis, angle);
492        let m_from_q = Mat4::from_quat(q_orig);
493
494        let q_from_m = Quaternion::from_rotation_matrix(&m_from_q);
495        let m_from_q_again = Mat4::from_quat(q_from_m);
496
497        // Compare original quaternion to the one extracted from matrix
498        assert!(quat_approx_eq(q_orig, q_from_m));
499
500        // Compare original matrix to the one rebuilt from the extracted quaternion
501        // This requires mat4_approx_eq from matrix tests
502        // assert!(mat4_approx_eq(m_from_q, m_from_q_again)); // Assuming mat4_approx_eq exists
503        // For now, just check rotation behaviour
504        let v = Vec3::new(1.0, 1.0, 1.0);
505        let v_rot_orig = m_from_q * Vec4::from_vec3(v, 1.0);
506        let v_rot_new = m_from_q_again * Vec4::from_vec3(v, 1.0);
507        assert_relative_eq!(v_rot_orig.x, v_rot_new.x, epsilon = EPSILON);
508        assert_relative_eq!(v_rot_orig.y, v_rot_new.y, epsilon = EPSILON);
509        assert_relative_eq!(v_rot_orig.z, v_rot_new.z, epsilon = EPSILON);
510    }
511
512    #[test]
513    fn test_conjugate_and_inverse_unit() {
514        let axis = Vec3::new(1.0, 2.0, 3.0).normalize();
515        let angle = 0.75;
516        let q = Quaternion::from_axis_angle(axis, angle);
517        let q_conj = q.conjugate();
518        let q_inv = q.inverse();
519
520        assert_relative_eq!(q_conj.x, q_inv.x, epsilon = EPSILON);
521        assert_relative_eq!(q_conj.y, q_inv.y, epsilon = EPSILON);
522        assert_relative_eq!(q_conj.z, q_inv.z, epsilon = EPSILON);
523        assert_relative_eq!(q_conj.w, q_inv.w, epsilon = EPSILON);
524
525        assert_relative_eq!(q_conj.x, -q.x, epsilon = EPSILON);
526        assert_relative_eq!(q_conj.y, -q.y, epsilon = EPSILON);
527        assert_relative_eq!(q_conj.z, -q.z, epsilon = EPSILON);
528        assert_relative_eq!(q_conj.w, q.w, epsilon = EPSILON);
529    }
530
531    #[test]
532    fn test_multiplication_identity() {
533        let axis = Vec3::Y;
534        let angle = std::f32::consts::FRAC_PI_2;
535        let q = Quaternion::from_axis_angle(axis, angle);
536
537        let res_qi = q * Quaternion::IDENTITY;
538        let res_iq = Quaternion::IDENTITY * q;
539
540        assert_relative_eq!(res_qi.x, q.x, epsilon = EPSILON);
541        assert_relative_eq!(res_qi.y, q.y, epsilon = EPSILON);
542        assert_relative_eq!(res_qi.z, q.z, epsilon = EPSILON);
543        assert_relative_eq!(res_qi.w, q.w, epsilon = EPSILON);
544
545        assert_relative_eq!(res_iq.x, q.x, epsilon = EPSILON);
546        assert_relative_eq!(res_iq.y, q.y, epsilon = EPSILON);
547        assert_relative_eq!(res_iq.z, q.z, epsilon = EPSILON);
548        assert_relative_eq!(res_iq.w, q.w, epsilon = EPSILON);
549    }
550
551    #[test]
552    fn test_multiplication_composition() {
553        let rot_y = Quaternion::from_axis_angle(Vec3::Y, std::f32::consts::FRAC_PI_2);
554        let rot_x = Quaternion::from_axis_angle(Vec3::X, std::f32::consts::FRAC_PI_2);
555        let combined_rot = rot_x * rot_y; // Y then X
556
557        let v_start = Vec3::Z;
558        let v_after_y = rot_y * v_start;
559        let v_after_x_then_y = rot_x * v_after_y;
560        let v_combined = combined_rot * v_start;
561
562        assert_relative_eq!(v_after_x_then_y.x, 1.0, epsilon = EPSILON);
563        assert_relative_eq!(v_after_x_then_y.y, 0.0, epsilon = EPSILON);
564        assert_relative_eq!(v_after_x_then_y.z, 0.0, epsilon = EPSILON);
565
566        assert_relative_eq!(v_combined.x, v_after_x_then_y.x, epsilon = EPSILON);
567        assert_relative_eq!(v_combined.y, v_after_x_then_y.y, epsilon = EPSILON);
568        assert_relative_eq!(v_combined.z, v_after_x_then_y.z, epsilon = EPSILON);
569    }
570
571    #[test]
572    fn test_multiplication_inverse() {
573        let axis = Vec3::new(1.0, -2.0, 0.5).normalize();
574        let angle = 1.2;
575        let q = Quaternion::from_axis_angle(axis, angle);
576        let q_inv = q.inverse();
577
578        let result_forward = q * q_inv;
579        let result_backward = q_inv * q;
580
581        assert_relative_eq!(result_forward.x, Quaternion::IDENTITY.x, epsilon = EPSILON);
582        assert_relative_eq!(result_forward.y, Quaternion::IDENTITY.y, epsilon = EPSILON);
583        assert_relative_eq!(result_forward.z, Quaternion::IDENTITY.z, epsilon = EPSILON);
584        assert_relative_eq!(result_forward.w, Quaternion::IDENTITY.w, epsilon = EPSILON);
585
586        assert_relative_eq!(result_backward.x, Quaternion::IDENTITY.x, epsilon = EPSILON);
587        assert_relative_eq!(result_backward.y, Quaternion::IDENTITY.y, epsilon = EPSILON);
588        assert_relative_eq!(result_backward.z, Quaternion::IDENTITY.z, epsilon = EPSILON);
589        assert_relative_eq!(result_backward.w, Quaternion::IDENTITY.w, epsilon = EPSILON);
590    }
591
592    #[test]
593    fn test_rotate_vec3_and_operator() {
594        let axis = Vec3::Y;
595        let angle = std::f32::consts::FRAC_PI_2;
596        let q = Quaternion::from_axis_angle(axis, angle);
597
598        let v_in = Vec3::X;
599        let v_out_method = q.rotate_vec3(v_in);
600        let v_out_operator = q * v_in;
601        let v_expected = Vec3::new(0.0, 0.0, -1.0);
602
603        assert_relative_eq!(v_out_method.x, v_expected.x, epsilon = EPSILON);
604        assert_relative_eq!(v_out_method.y, v_expected.y, epsilon = EPSILON);
605        assert_relative_eq!(v_out_method.z, v_expected.z, epsilon = EPSILON);
606
607        assert_relative_eq!(v_out_operator.x, v_expected.x, epsilon = EPSILON);
608        assert_relative_eq!(v_out_operator.y, v_expected.y, epsilon = EPSILON);
609        assert_relative_eq!(v_out_operator.z, v_expected.z, epsilon = EPSILON);
610    }
611
612    #[test]
613    fn test_normalization() {
614        let q_non_unit = Quaternion::new(1.0, 2.0, 3.0, 4.0);
615        let q_norm = q_non_unit.normalize();
616        assert_relative_eq!(q_norm.magnitude(), 1.0, epsilon = EPSILON);
617
618        let q_mut = q_non_unit;
619        let q_mut = q_mut.normalize();
620        assert_relative_eq!(q_mut.magnitude(), 1.0, epsilon = EPSILON);
621
622        assert_relative_eq!(q_mut.x, q_norm.x, epsilon = EPSILON);
623        assert_relative_eq!(q_mut.y, q_norm.y, epsilon = EPSILON);
624        assert_relative_eq!(q_mut.z, q_norm.z, epsilon = EPSILON);
625        assert_relative_eq!(q_mut.w, q_norm.w, epsilon = EPSILON);
626    }
627
628    #[test]
629    fn test_normalize_zero_quaternion() {
630        let q_zero = Quaternion::new(0.0, 0.0, 0.0, 0.0);
631        let q_norm = q_zero.normalize();
632        assert_eq!(q_norm, Quaternion::IDENTITY);
633    }
634
635    #[test]
636    fn test_dot_product() {
637        let angle = 0.5;
638        let q1 = Quaternion::from_axis_angle(Vec3::X, angle);
639        let q2 = Quaternion::from_axis_angle(Vec3::X, angle);
640        let q3 = Quaternion::from_axis_angle(Vec3::Y, angle);
641        let q4 = Quaternion::from_axis_angle(Vec3::X, -angle);
642
643        assert_relative_eq!(q1.dot(q1), 1.0, epsilon = EPSILON);
644        assert_relative_eq!(q1.dot(q2), 1.0, epsilon = EPSILON);
645        assert!(q1.dot(q3).abs() < 1.0 - EPSILON);
646        assert_relative_eq!(q1.dot(q4), angle.cos(), epsilon = EPSILON);
647    }
648
649    #[test]
650    fn test_slerp_endpoints() {
651        let q_start = Quaternion::IDENTITY;
652        let q_end = Quaternion::from_axis_angle(Vec3::Z, std::f32::consts::FRAC_PI_2);
653
654        let q_t0 = Quaternion::slerp(q_start, q_end, 0.0);
655        let q_t1 = Quaternion::slerp(q_start, q_end, 1.0);
656
657        assert_relative_eq!(q_t0.x, q_start.x, epsilon = EPSILON);
658        assert_relative_eq!(q_t0.y, q_start.y, epsilon = EPSILON);
659        assert_relative_eq!(q_t0.z, q_start.z, epsilon = EPSILON);
660        assert_relative_eq!(q_t0.w, q_start.w, epsilon = EPSILON);
661
662        assert_relative_eq!(q_t1.x, q_end.x, epsilon = EPSILON);
663        assert_relative_eq!(q_t1.y, q_end.y, epsilon = EPSILON);
664        assert_relative_eq!(q_t1.z, q_end.z, epsilon = EPSILON);
665        assert_relative_eq!(q_t1.w, q_end.w, epsilon = EPSILON);
666    }
667
668    #[test]
669    fn test_slerp_midpoint() {
670        let q_start = Quaternion::IDENTITY;
671        let q_end = Quaternion::from_axis_angle(Vec3::Z, std::f32::consts::FRAC_PI_2);
672        let q_half = Quaternion::slerp(q_start, q_end, 0.5);
673        let q_expected_half =
674            Quaternion::from_axis_angle(Vec3::Z, std::f32::consts::FRAC_PI_2 * 0.5);
675
676        assert_relative_eq!(q_half.x, q_expected_half.x, epsilon = EPSILON);
677        assert_relative_eq!(q_half.y, q_expected_half.y, epsilon = EPSILON);
678        assert_relative_eq!(q_half.z, q_expected_half.z, epsilon = EPSILON);
679        assert_relative_eq!(q_half.w, q_expected_half.w, epsilon = EPSILON);
680        assert_relative_eq!(q_half.magnitude(), 1.0, epsilon = EPSILON);
681    }
682
683    #[test]
684    fn test_slerp_short_path_handling() {
685        let q_start = Quaternion::from_axis_angle(Vec3::Y, -30.0f32.to_radians());
686        let q_end = Quaternion::from_axis_angle(Vec3::Y, 170.0f32.to_radians());
687        assert!(q_start.dot(q_end) < 0.0);
688
689        let q_mid = Quaternion::slerp(q_start, q_end, 0.5);
690        let q_expected_mid = Quaternion::from_axis_angle(Vec3::Y, -110.0f32.to_radians()); // Midpoint on shortest path
691
692        assert_relative_eq!(q_mid.dot(q_expected_mid).abs(), 1.0, epsilon = EPSILON);
693
694        let v = Vec3::X;
695        let v_rotated_mid = q_mid * v;
696        let v_rotated_expected = q_expected_mid * v;
697        assert_relative_eq!(v_rotated_mid.x, v_rotated_expected.x, epsilon = EPSILON);
698        assert_relative_eq!(v_rotated_mid.y, v_rotated_expected.y, epsilon = EPSILON);
699        assert_relative_eq!(v_rotated_mid.z, v_rotated_expected.z, epsilon = EPSILON);
700    }
701
702    #[test]
703    fn test_slerp_near_identical_quaternions() {
704        let angle1 = 0.00001;
705        let angle2 = 0.00002;
706        let q_close1 = Quaternion::from_axis_angle(Vec3::Y, angle1);
707        let q_close2 = Quaternion::from_axis_angle(Vec3::Y, angle2);
708        assert!(q_close1.dot(q_close2) > 1.0 - EPSILON);
709
710        let q_mid = Quaternion::slerp(q_close1, q_close2, 0.5);
711        let angle_mid = angle1 + (angle2 - angle1) * 0.5;
712        let q_expected = Quaternion::from_axis_angle(Vec3::Y, angle_mid);
713
714        assert_relative_eq!(q_mid.magnitude(), 1.0, epsilon = EPSILON * 10.0);
715
716        let v = Vec3::X;
717        let v_rotated = q_mid * v;
718        let v_expected_rotated = q_expected * v;
719        assert_relative_eq!(v_rotated.x, v_expected_rotated.x, epsilon = EPSILON * 10.0);
720        assert_relative_eq!(v_rotated.y, v_expected_rotated.y, epsilon = EPSILON * 10.0);
721        assert_relative_eq!(v_rotated.z, v_expected_rotated.z, epsilon = EPSILON * 10.0);
722    }
723
724    #[test]
725    fn test_slerp_clamps_t() {
726        let q_start = Quaternion::IDENTITY;
727        let q_end = Quaternion::from_axis_angle(Vec3::Z, std::f32::consts::FRAC_PI_2);
728
729        let q_t_neg = Quaternion::slerp(q_start, q_end, -0.5); // t < 0
730        let q_t_large = Quaternion::slerp(q_start, q_end, 1.5); // t > 1
731
732        assert_relative_eq!(q_t_neg.x, q_start.x, epsilon = EPSILON);
733        assert_relative_eq!(q_t_neg.y, q_start.y, epsilon = EPSILON);
734        assert_relative_eq!(q_t_neg.z, q_start.z, epsilon = EPSILON);
735        assert_relative_eq!(q_t_neg.w, q_start.w, epsilon = EPSILON);
736
737        assert_relative_eq!(q_t_large.x, q_end.x, epsilon = EPSILON);
738        assert_relative_eq!(q_t_large.y, q_end.y, epsilon = EPSILON);
739        assert_relative_eq!(q_t_large.z, q_end.z, epsilon = EPSILON);
740        assert_relative_eq!(q_t_large.w, q_end.w, epsilon = EPSILON);
741    }
742}
khora_core/math/quaternion.rs

khora_core/math/
quaternion.rs
