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

khora_core/math/
quaternion.rs
