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

khora_core/math/
quaternion.rs
