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

khora_core/math/
quaternion.rs
