1#![cfg(feature = "unstable")]
2
3use core::ops::{Add, Mul, Neg};
4
5use num_traits::{ConstOne, ConstZero, Inv, Num, One, Zero};
6
7#[cfg(any(feature = "std", feature = "libm"))]
8use num_traits::{Float, FloatConst};
9
10#[cfg(any(feature = "std", feature = "libm"))]
11use crate::UnitQuaternion;
12
13#[derive(Clone, Copy, Debug, Default, Eq, Hash, PartialEq)]
30pub struct PureQuaternion<T> {
31 pub x: T,
33 pub y: T,
35 pub z: T,
37}
38
39pub type PQ32 = PureQuaternion<f32>;
41pub type PQ64 = PureQuaternion<f64>;
43
44impl<T> PureQuaternion<T> {
45 #[inline]
54 pub const fn new(x: T, y: T, z: T) -> Self {
55 Self { x, y, z }
56 }
57}
58
59impl<T> PureQuaternion<T>
60where
61 T: ConstZero,
62{
63 pub const ZERO: Self = Self::new(T::ZERO, T::ZERO, T::ZERO);
73}
74
75impl<T> ConstZero for PureQuaternion<T>
76where
77 T: ConstZero,
78{
79 const ZERO: Self = Self::ZERO;
80}
81
82impl<T> Zero for PureQuaternion<T>
83where
84 T: Zero,
85{
86 #[inline]
87 fn zero() -> Self {
88 Self::new(T::zero(), T::zero(), T::zero())
89 }
90
91 #[inline]
92 fn is_zero(&self) -> bool {
93 self.x.is_zero() && self.y.is_zero() && self.z.is_zero()
94 }
95
96 #[inline]
97 fn set_zero(&mut self) {
98 self.x.set_zero();
99 self.y.set_zero();
100 self.z.set_zero();
101 }
102}
103
104impl<T> PureQuaternion<T>
105where
106 T: ConstZero + ConstOne,
107{
108 pub const I: Self = Self::new(T::ONE, T::ZERO, T::ZERO);
120
121 pub const J: Self = Self::new(T::ZERO, T::ONE, T::ZERO);
133
134 pub const K: Self = Self::new(T::ZERO, T::ZERO, T::ONE);
146}
147
148impl<T> PureQuaternion<T>
149where
150 T: Zero + One,
151{
152 #[inline]
164 pub fn i() -> Self {
165 Self::new(T::one(), T::zero(), T::zero())
166 }
167
168 #[inline]
180 pub fn j() -> Self {
181 Self::new(T::zero(), T::one(), T::zero())
182 }
183
184 #[inline]
196 pub fn k() -> Self {
197 Self::new(T::zero(), T::zero(), T::one())
198 }
199}
200
201#[cfg(any(feature = "std", feature = "libm"))]
202impl<T> PureQuaternion<T>
203where
204 T: Float,
205{
206 #[inline]
218 pub fn nan() -> Self {
219 let nan = T::nan();
220 Self::new(nan, nan, nan)
221 }
222}
223
224impl<T> PureQuaternion<T>
225where
226 T: Clone + Mul<T, Output = T> + Add<T, Output = T>,
227{
228 #[inline]
247 pub fn norm_sqr(&self) -> T {
248 self.x.clone() * self.x.clone()
249 + self.y.clone() * self.y.clone()
250 + self.z.clone() * self.z.clone()
251 }
252}
253
254impl<T> PureQuaternion<T>
255where
256 T: Clone + Neg<Output = T>,
257{
258 #[inline]
268 pub fn conj(&self) -> Self {
269 Self::new(-self.x.clone(), -self.y.clone(), -self.z.clone())
270 }
271}
272
273impl<T> PureQuaternion<T>
274where
275 for<'a> &'a Self: Inv<Output = PureQuaternion<T>>,
276{
277 #[inline]
288 pub fn inv(&self) -> Self {
289 Inv::inv(self)
290 }
291}
292
293impl<T> Inv for &PureQuaternion<T>
294where
295 T: Clone + Neg<Output = T> + Num,
296{
297 type Output = PureQuaternion<T>;
298
299 #[inline]
300 fn inv(self) -> Self::Output {
301 let norm_sqr = self.norm_sqr();
302 PureQuaternion::new(
303 -self.x.clone() / norm_sqr.clone(),
304 -self.y.clone() / norm_sqr.clone(),
305 -self.z.clone() / norm_sqr,
306 )
307 }
308}
309
310impl<T> Inv for PureQuaternion<T>
311where
312 for<'a> &'a Self: Inv<Output = PureQuaternion<T>>,
313{
314 type Output = PureQuaternion<T>;
315
316 #[inline]
317 fn inv(self) -> Self::Output {
318 Inv::inv(&self)
319 }
320}
321
322#[cfg(any(feature = "std", feature = "libm"))]
323impl<T> PureQuaternion<T>
324where
325 T: Float,
326{
327 #[inline]
345 pub fn norm(self) -> T {
346 let one = T::one();
347 let two = one + one;
348 let s = T::min_positive_value();
349 let norm_sqr = self.norm_sqr();
350 if norm_sqr < T::infinity() {
351 if norm_sqr >= s * two {
352 norm_sqr.sqrt()
353 } else if self.is_zero() {
354 T::zero()
357 } else {
358 (self / s).fast_norm() * s
361 }
362 } else {
363 (self * s).fast_norm() / s
374 }
375 }
376}
377
378#[cfg(any(feature = "std", feature = "libm"))]
379impl<T> PureQuaternion<T>
380where
381 T: Float,
382{
383 #[inline]
413 pub fn fast_norm(self) -> T {
414 self.norm_sqr().sqrt()
415 }
416}
417
418#[cfg(any(feature = "std", feature = "libm"))]
419impl<T> PureQuaternion<T>
420where
421 T: Float + FloatConst,
422{
423 pub fn exp(self) -> UnitQuaternion<T> {
444 let one = T::one();
445
446 let sqr_angle = self.x * self.x + self.y * self.y + self.z * self.z;
448
449 if sqr_angle <= T::epsilon() {
450 UnitQuaternion::new(one, self.x, self.y, self.z)
460 } else {
461 let angle = sqr_angle.sqrt();
463 let cos_angle = angle.cos();
464 let sinc_angle = angle.sin() / angle;
465 let w = cos_angle;
466 let x = self.x * sinc_angle;
467 let y = self.y * sinc_angle;
468 let z = self.z * sinc_angle;
469 UnitQuaternion::new(w, x, y, z)
470 }
471 }
472}
473
474#[cfg(feature = "serde")]
475impl<T> serde::Serialize for PureQuaternion<T>
476where
477 T: serde::Serialize,
478{
479 fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
480 where
481 S: serde::Serializer,
482 {
483 (&self.x, &self.y, &self.z).serialize(serializer)
484 }
485}
486
487#[cfg(feature = "serde")]
488impl<'de, T> serde::Deserialize<'de> for PureQuaternion<T>
489where
490 T: serde::Deserialize<'de>,
491{
492 fn deserialize<D>(deserializer: D) -> Result<Self, D::Error>
493 where
494 D: serde::Deserializer<'de>,
495 {
496 let (x, y, z) = serde::Deserialize::deserialize(deserializer)?;
497 Ok(PureQuaternion::new(x, y, z))
498 }
499}
500
501#[cfg(test)]
502mod tests {
503 #[cfg(any(feature = "std", feature = "libm"))]
504 use core::f32;
505
506 use super::*;
507
508 #[test]
509 fn test_new() {
510 let q = PQ32::new(1.0, 2.0, 3.0);
511 assert_eq!(q.x, 1.0);
512 assert_eq!(q.y, 2.0);
513 assert_eq!(q.z, 3.0);
514 }
515
516 #[test]
517 fn test_zero_const() {
518 let q = PQ64::ZERO;
519 assert_eq!(q.x, 0.0);
520 assert_eq!(q.y, 0.0);
521 assert_eq!(q.z, 0.0);
522 }
523
524 #[test]
525 fn test_const_zero() {
526 let q: PQ32 = ConstZero::ZERO;
527 assert_eq!(q.x, 0.0);
528 assert_eq!(q.y, 0.0);
529 assert_eq!(q.z, 0.0);
530 }
531
532 #[test]
533 fn test_zero_trait() {
534 let q = PQ32::zero();
535 assert_eq!(q.x, 0.0);
536 assert_eq!(q.y, 0.0);
537 assert_eq!(q.z, 0.0);
538 assert!(q.is_zero());
539
540 let mut q = PQ32::new(1.0, 2.0, 3.0);
541 assert!(!q.is_zero());
542 q.set_zero();
543 assert!(q.is_zero());
544 }
545
546 #[test]
547 fn test_i_const() {
548 let q = PQ32::I;
549 assert_eq!(q.x, 1.0);
550 assert_eq!(q.y, 0.0);
551 assert_eq!(q.z, 0.0);
552 }
553
554 #[test]
555 fn test_j_const() {
556 let q = PQ64::J;
557 assert_eq!(q.x, 0.0);
558 assert_eq!(q.y, 1.0);
559 assert_eq!(q.z, 0.0);
560 }
561
562 #[test]
563 fn test_k_const() {
564 let q = PQ32::K;
565 assert_eq!(q.x, 0.0);
566 assert_eq!(q.y, 0.0);
567 assert_eq!(q.z, 1.0);
568 }
569
570 #[test]
571 fn test_i_static() {
572 let q = PQ64::i();
573 assert_eq!(q.x, 1.0);
574 assert_eq!(q.y, 0.0);
575 assert_eq!(q.z, 0.0);
576 }
577
578 #[test]
579 fn test_j_static() {
580 let q = PQ32::j();
581 assert_eq!(q.x, 0.0);
582 assert_eq!(q.y, 1.0);
583 assert_eq!(q.z, 0.0);
584 }
585
586 #[test]
587 fn test_k_static() {
588 let q = PQ64::k();
589 assert_eq!(q.x, 0.0);
590 assert_eq!(q.y, 0.0);
591 assert_eq!(q.z, 1.0);
592 }
593
594 #[cfg(any(feature = "std", feature = "libm"))]
595 #[test]
596 fn test_nan() {
597 let q = PQ32::nan();
598 assert!(q.x.is_nan());
599 assert!(q.y.is_nan());
600 assert!(q.z.is_nan());
601 }
602
603 #[test]
604 fn test_norm_sqr() {
605 let q = PQ64::new(1.0, 2.0, 3.0);
606 assert_eq!(q.norm_sqr(), 14.0);
607 }
608
609 #[test]
610 fn test_conj() {
611 let q = PQ32::new(1.0, 2.0, 3.0);
612 assert_eq!(q.conj(), PureQuaternion::new(-1.0, -2.0, -3.0));
613 }
614
615 #[test]
616 fn test_inv() {
617 let q = PQ64::new(1.0, 2.0, 3.0);
618 assert_eq!(
619 q.inv(),
620 PureQuaternion::new(-1.0 / 14.0, -2.0 / 14.0, -3.0 / 14.0)
621 );
622 }
623
624 #[test]
625 fn test_inv_trait() {
626 let q = PQ32::new(1.0, 2.0, 3.0);
627 assert_eq!(
628 Inv::inv(&q),
629 PureQuaternion::new(-1.0 / 14.0, -2.0 / 14.0, -3.0 / 14.0)
630 );
631 }
632
633 #[cfg(any(feature = "std", feature = "libm"))]
634 #[test]
635 fn test_norm_normal_values() {
636 let q = PQ64::new(1.0, 2.0, 3.0);
637 assert_eq!(q.norm(), 14.0f64.sqrt());
638 }
639
640 #[cfg(any(feature = "std", feature = "libm"))]
641 #[test]
642 fn test_norm_zero_quaternion() {
643 let q = PQ32::zero();
644 assert_eq!(q.norm(), 0.0);
645 }
646
647 #[cfg(any(feature = "std", feature = "libm"))]
648 #[test]
649 fn test_norm_subnormal_values() {
650 let s = f64::MIN_POSITIVE * 0.25;
651 let q = PQ64::new(s, s, s);
652 assert!(
653 (q.norm() - s * 3.0f64.sqrt()).abs() < 4.0 * s * f64::EPSILON,
654 "Norm of subnormal quaternion is not accurate."
655 )
656 }
657
658 #[cfg(any(feature = "std", feature = "libm"))]
659 #[test]
660 fn test_norm_large_values() {
661 let s = f64::MAX * 0.5;
662 let q = PQ64::new(s, s, s);
663 assert!(
664 (q.norm() - s * 3.0f64.sqrt()).abs() < 2.0 * s * f64::EPSILON,
665 "Norm of large quaternion is not accurate."
666 );
667 }
668
669 #[cfg(any(feature = "std", feature = "libm"))]
670 #[test]
671 fn test_norm_infinite_values() {
672 let inf = f32::INFINITY;
673 assert_eq!(PQ32::new(inf, 1.0, 1.0).norm(), inf);
674 assert_eq!(PQ32::new(1.0, inf, 1.0).norm(), inf);
675 assert_eq!(PQ32::new(1.0, 1.0, inf).norm(), inf);
676 }
677
678 #[cfg(any(feature = "std", feature = "libm"))]
679 #[test]
680 fn test_norm_nan_values() {
681 let nan = f32::NAN;
682 assert!(PQ32::new(nan, 1.0, 1.0).norm().is_nan());
683 assert!(PQ32::new(1.0, nan, 1.0).norm().is_nan());
684 assert!(PQ32::new(1.0, 1.0, nan).norm().is_nan());
685 }
686
687 #[cfg(any(feature = "std", feature = "libm"))]
688 #[test]
689 fn test_fast_norm_normal_values() {
690 let q = PQ64::new(1.1, 2.7, 3.4);
691 assert_eq!(q.fast_norm(), q.norm());
692 }
693
694 #[cfg(any(feature = "std", feature = "libm"))]
695 #[test]
696 fn test_fast_norm_zero_quaternion() {
697 let q = PQ32::zero();
698 assert_eq!(q.fast_norm(), 0.0);
699 }
700
701 #[cfg(any(feature = "std", feature = "libm"))]
702 #[test]
703 fn test_fast_norm_infinite_values() {
704 let inf = f32::INFINITY;
705 assert_eq!(PQ32::new(inf, 1.0, 1.0).fast_norm(), inf);
706 assert_eq!(PQ32::new(1.0, inf, 1.0).fast_norm(), inf);
707 assert_eq!(PQ32::new(1.0, 1.0, inf).fast_norm(), inf);
708 }
709
710 #[cfg(any(feature = "std", feature = "libm"))]
711 #[test]
712 fn test_fast_norm_nan_values() {
713 let nan = f32::NAN;
714 assert!(PQ32::new(nan, 1.0, 1.0).fast_norm().is_nan());
715 assert!(PQ32::new(1.0, nan, 1.0).fast_norm().is_nan());
716 assert!(PQ32::new(1.0, 1.0, nan).fast_norm().is_nan());
717 }
718
719 #[cfg(any(feature = "std", feature = "libm"))]
720 #[test]
721 fn test_exp_zero_quaternion() {
722 assert_eq!(PQ64::ZERO.exp(), UnitQuaternion::ONE);
723 }
724
725 #[cfg(any(feature = "std", feature = "libm"))]
726 #[test]
727 fn test_exp_i_quaternion() {
728 let q = PQ32::I;
729 let exp_q = q.exp();
730 let expected =
731 UnitQuaternion::new(1.0f32.cos(), 1.0f32.sin(), 0.0, 0.0);
732 assert_eq!(exp_q, expected);
733 }
734
735 #[cfg(any(feature = "std", feature = "libm"))]
736 #[test]
737 fn test_exp_complex_quaternion() {
738 let q = PQ64::new(1.0, 1.0, 1.0);
739 let exp_q = q.exp();
740 let angle = 3.0f64.sqrt();
741 let re = angle.cos();
742 let im = angle.sin() / angle;
743 let expected = UnitQuaternion::new(re, im, im, im);
744 assert!((exp_q - expected).norm() <= 2.0 * f64::EPSILON);
745 }
746
747 #[cfg(any(feature = "std", feature = "libm"))]
748 #[test]
749 fn test_exp_nan_quaternion() {
750 for q in [
751 PQ32::new(f32::NAN, 1.0, 1.0),
752 PQ32::new(1.0, f32::NAN, 1.0),
753 PQ32::new(1.0, 1.0, f32::NAN),
754 ]
755 .iter()
756 {
757 let exp_q = q.exp();
758 assert!(exp_q.as_quaternion().w.is_nan());
759 assert!(exp_q.as_quaternion().x.is_nan());
760 assert!(exp_q.as_quaternion().y.is_nan());
761 assert!(exp_q.as_quaternion().z.is_nan());
762 }
763 }
764
765 #[cfg(any(feature = "std", feature = "libm"))]
766 #[test]
767 fn test_exp_large_imaginary_norm() {
768 let q = PQ32::new(1e30, 1e30, 1e30);
769 let exp_q = q.exp();
770 assert!(exp_q.as_quaternion().w.is_nan());
771 assert!(exp_q.as_quaternion().x.is_nan());
772 assert!(exp_q.as_quaternion().y.is_nan());
773 assert!(exp_q.as_quaternion().z.is_nan());
774 }
775
776 #[cfg(any(feature = "std", feature = "libm"))]
777 #[test]
778 fn test_exp_infinite_imaginary_part() {
779 let q = PQ64::new(1.0, 1.0, f64::INFINITY);
780 let exp_q = q.exp();
781 assert!(exp_q.as_quaternion().w.is_nan());
782 assert!(exp_q.as_quaternion().x.is_nan());
783 assert!(exp_q.as_quaternion().y.is_nan());
784 assert!(exp_q.as_quaternion().z.is_nan());
785 }
786
787 #[cfg(any(feature = "std", feature = "libm"))]
788 #[test]
789 fn test_exp_small_imaginary_norm() {
790 let epsilon = f32::EPSILON;
791 let q = PQ32::new(epsilon, epsilon, epsilon);
792 let exp_q = q.exp();
793 let expected = UnitQuaternion::new(1.0, epsilon, epsilon, epsilon);
794 assert!((exp_q - expected).norm() <= 0.5 * f32::EPSILON);
795 }
796
797 #[cfg(feature = "serde")]
798 #[test]
799 fn test_serde_pure_quaternion() {
800 let q = PQ32::new(1.0, 2.0, 3.0);
801 let serialized =
802 serde_json::to_string(&q).expect("Failed to serialize quaternion");
803
804 let deserialized: PQ32 = serde_json::from_str(&serialized)
805 .expect("Failed to deserialize quaternion");
806 assert_eq!(deserialized, q);
807 }
808}