#![cfg(feature = "unstable")]
use core::ops::{Add, Mul, Neg};
use num_traits::{ConstOne, ConstZero, Inv, Num, One, Zero};
#[cfg(any(feature = "std", feature = "libm"))]
use num_traits::{Float, FloatConst};
#[cfg(any(feature = "std", feature = "libm"))]
use crate::UnitQuaternion;
#[derive(Clone, Copy, Debug, Default, Eq, Hash, PartialEq)]
pub struct PureQuaternion<T> {
pub x: T,
pub y: T,
pub z: T,
}
pub type PQ32 = PureQuaternion<f32>;
pub type PQ64 = PureQuaternion<f64>;
impl<T> PureQuaternion<T> {
#[inline]
pub const fn new(x: T, y: T, z: T) -> Self {
Self { x, y, z }
}
}
impl<T> PureQuaternion<T>
where
T: ConstZero,
{
pub const ZERO: Self = Self::new(T::ZERO, T::ZERO, T::ZERO);
}
impl<T> ConstZero for PureQuaternion<T>
where
T: ConstZero,
{
const ZERO: Self = Self::ZERO;
}
impl<T> Zero for PureQuaternion<T>
where
T: Zero,
{
#[inline]
fn zero() -> Self {
Self::new(T::zero(), T::zero(), T::zero())
}
#[inline]
fn is_zero(&self) -> bool {
self.x.is_zero() && self.y.is_zero() && self.z.is_zero()
}
#[inline]
fn set_zero(&mut self) {
self.x.set_zero();
self.y.set_zero();
self.z.set_zero();
}
}
impl<T> PureQuaternion<T>
where
T: ConstZero + ConstOne,
{
pub const I: Self = Self::new(T::ONE, T::ZERO, T::ZERO);
pub const J: Self = Self::new(T::ZERO, T::ONE, T::ZERO);
pub const K: Self = Self::new(T::ZERO, T::ZERO, T::ONE);
}
impl<T> PureQuaternion<T>
where
T: Zero + One,
{
#[inline]
pub fn i() -> Self {
Self::new(T::one(), T::zero(), T::zero())
}
#[inline]
pub fn j() -> Self {
Self::new(T::zero(), T::one(), T::zero())
}
#[inline]
pub fn k() -> Self {
Self::new(T::zero(), T::zero(), T::one())
}
}
#[cfg(any(feature = "std", feature = "libm"))]
impl<T> PureQuaternion<T>
where
T: Float,
{
#[inline]
pub fn nan() -> Self {
let nan = T::nan();
Self::new(nan, nan, nan)
}
}
impl<T> PureQuaternion<T>
where
T: Clone + Mul<T, Output = T> + Add<T, Output = T>,
{
#[inline]
pub fn norm_sqr(&self) -> T {
self.x.clone() * self.x.clone()
+ self.y.clone() * self.y.clone()
+ self.z.clone() * self.z.clone()
}
}
impl<T> PureQuaternion<T>
where
T: Clone + Neg<Output = T>,
{
#[inline]
pub fn conj(&self) -> Self {
Self::new(-self.x.clone(), -self.y.clone(), -self.z.clone())
}
}
impl<T> PureQuaternion<T>
where
for<'a> &'a Self: Inv<Output = PureQuaternion<T>>,
{
#[inline]
pub fn inv(&self) -> Self {
Inv::inv(self)
}
}
impl<T> Inv for &PureQuaternion<T>
where
T: Clone + Neg<Output = T> + Num,
{
type Output = PureQuaternion<T>;
#[inline]
fn inv(self) -> Self::Output {
let norm_sqr = self.norm_sqr();
PureQuaternion::new(
-self.x.clone() / norm_sqr.clone(),
-self.y.clone() / norm_sqr.clone(),
-self.z.clone() / norm_sqr,
)
}
}
impl<T> Inv for PureQuaternion<T>
where
for<'a> &'a Self: Inv<Output = PureQuaternion<T>>,
{
type Output = PureQuaternion<T>;
#[inline]
fn inv(self) -> Self::Output {
Inv::inv(&self)
}
}
#[cfg(any(feature = "std", feature = "libm"))]
impl<T> PureQuaternion<T>
where
T: Float,
{
#[inline]
pub fn norm(self) -> T {
let one = T::one();
let two = one + one;
let s = T::min_positive_value();
let norm_sqr = self.norm_sqr();
if norm_sqr < T::infinity() {
if norm_sqr >= s * two {
norm_sqr.sqrt()
} else if self.is_zero() {
T::zero()
} else {
(self / s).fast_norm() * s
}
} else {
(self * s).fast_norm() / s
}
}
}
#[cfg(any(feature = "std", feature = "libm"))]
impl<T> PureQuaternion<T>
where
T: Float,
{
#[inline]
pub fn fast_norm(self) -> T {
self.norm_sqr().sqrt()
}
}
#[cfg(any(feature = "std", feature = "libm"))]
impl<T> PureQuaternion<T>
where
T: Float + FloatConst,
{
pub fn exp(self) -> UnitQuaternion<T> {
let one = T::one();
let sqr_angle = self.x * self.x + self.y * self.y + self.z * self.z;
if sqr_angle <= T::epsilon() {
UnitQuaternion::new(one, self.x, self.y, self.z)
} else {
let angle = sqr_angle.sqrt();
let cos_angle = angle.cos();
let sinc_angle = angle.sin() / angle;
let w = cos_angle;
let x = self.x * sinc_angle;
let y = self.y * sinc_angle;
let z = self.z * sinc_angle;
UnitQuaternion::new(w, x, y, z)
}
}
}
#[cfg(feature = "serde")]
impl<T> serde::Serialize for PureQuaternion<T>
where
T: serde::Serialize,
{
fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
where
S: serde::Serializer,
{
(&self.x, &self.y, &self.z).serialize(serializer)
}
}
#[cfg(feature = "serde")]
impl<'de, T> serde::Deserialize<'de> for PureQuaternion<T>
where
T: serde::Deserialize<'de>,
{
fn deserialize<D>(deserializer: D) -> Result<Self, D::Error>
where
D: serde::Deserializer<'de>,
{
let (x, y, z) = serde::Deserialize::deserialize(deserializer)?;
Ok(PureQuaternion::new(x, y, z))
}
}
#[cfg(test)]
mod tests {
#[cfg(any(feature = "std", feature = "libm"))]
use core::f32;
use super::*;
#[test]
fn test_new() {
let q = PQ32::new(1.0, 2.0, 3.0);
assert_eq!(q.x, 1.0);
assert_eq!(q.y, 2.0);
assert_eq!(q.z, 3.0);
}
#[test]
fn test_zero_const() {
let q = PQ64::ZERO;
assert_eq!(q.x, 0.0);
assert_eq!(q.y, 0.0);
assert_eq!(q.z, 0.0);
}
#[test]
fn test_const_zero() {
let q: PQ32 = ConstZero::ZERO;
assert_eq!(q.x, 0.0);
assert_eq!(q.y, 0.0);
assert_eq!(q.z, 0.0);
}
#[test]
fn test_zero_trait() {
let q = PQ32::zero();
assert_eq!(q.x, 0.0);
assert_eq!(q.y, 0.0);
assert_eq!(q.z, 0.0);
assert!(q.is_zero());
let mut q = PQ32::new(1.0, 2.0, 3.0);
assert!(!q.is_zero());
q.set_zero();
assert!(q.is_zero());
}
#[test]
fn test_i_const() {
let q = PQ32::I;
assert_eq!(q.x, 1.0);
assert_eq!(q.y, 0.0);
assert_eq!(q.z, 0.0);
}
#[test]
fn test_j_const() {
let q = PQ64::J;
assert_eq!(q.x, 0.0);
assert_eq!(q.y, 1.0);
assert_eq!(q.z, 0.0);
}
#[test]
fn test_k_const() {
let q = PQ32::K;
assert_eq!(q.x, 0.0);
assert_eq!(q.y, 0.0);
assert_eq!(q.z, 1.0);
}
#[test]
fn test_i_static() {
let q = PQ64::i();
assert_eq!(q.x, 1.0);
assert_eq!(q.y, 0.0);
assert_eq!(q.z, 0.0);
}
#[test]
fn test_j_static() {
let q = PQ32::j();
assert_eq!(q.x, 0.0);
assert_eq!(q.y, 1.0);
assert_eq!(q.z, 0.0);
}
#[test]
fn test_k_static() {
let q = PQ64::k();
assert_eq!(q.x, 0.0);
assert_eq!(q.y, 0.0);
assert_eq!(q.z, 1.0);
}
#[cfg(any(feature = "std", feature = "libm"))]
#[test]
fn test_nan() {
let q = PQ32::nan();
assert!(q.x.is_nan());
assert!(q.y.is_nan());
assert!(q.z.is_nan());
}
#[test]
fn test_norm_sqr() {
let q = PQ64::new(1.0, 2.0, 3.0);
assert_eq!(q.norm_sqr(), 14.0);
}
#[test]
fn test_conj() {
let q = PQ32::new(1.0, 2.0, 3.0);
assert_eq!(q.conj(), PureQuaternion::new(-1.0, -2.0, -3.0));
}
#[test]
fn test_inv() {
let q = PQ64::new(1.0, 2.0, 3.0);
assert_eq!(
q.inv(),
PureQuaternion::new(-1.0 / 14.0, -2.0 / 14.0, -3.0 / 14.0)
);
}
#[test]
fn test_inv_trait() {
let q = PQ32::new(1.0, 2.0, 3.0);
assert_eq!(
Inv::inv(&q),
PureQuaternion::new(-1.0 / 14.0, -2.0 / 14.0, -3.0 / 14.0)
);
}
#[cfg(any(feature = "std", feature = "libm"))]
#[test]
fn test_norm_normal_values() {
let q = PQ64::new(1.0, 2.0, 3.0);
assert_eq!(q.norm(), 14.0f64.sqrt());
}
#[cfg(any(feature = "std", feature = "libm"))]
#[test]
fn test_norm_zero_quaternion() {
let q = PQ32::zero();
assert_eq!(q.norm(), 0.0);
}
#[cfg(any(feature = "std", feature = "libm"))]
#[test]
fn test_norm_subnormal_values() {
let s = f64::MIN_POSITIVE * 0.25;
let q = PQ64::new(s, s, s);
assert!(
(q.norm() - s * 3.0f64.sqrt()).abs() < 4.0 * s * f64::EPSILON,
"Norm of subnormal quaternion is not accurate."
)
}
#[cfg(any(feature = "std", feature = "libm"))]
#[test]
fn test_norm_large_values() {
let s = f64::MAX * 0.5;
let q = PQ64::new(s, s, s);
assert!(
(q.norm() - s * 3.0f64.sqrt()).abs() < 2.0 * s * f64::EPSILON,
"Norm of large quaternion is not accurate."
);
}
#[cfg(any(feature = "std", feature = "libm"))]
#[test]
fn test_norm_infinite_values() {
let inf = f32::INFINITY;
assert_eq!(PQ32::new(inf, 1.0, 1.0).norm(), inf);
assert_eq!(PQ32::new(1.0, inf, 1.0).norm(), inf);
assert_eq!(PQ32::new(1.0, 1.0, inf).norm(), inf);
}
#[cfg(any(feature = "std", feature = "libm"))]
#[test]
fn test_norm_nan_values() {
let nan = f32::NAN;
assert!(PQ32::new(nan, 1.0, 1.0).norm().is_nan());
assert!(PQ32::new(1.0, nan, 1.0).norm().is_nan());
assert!(PQ32::new(1.0, 1.0, nan).norm().is_nan());
}
#[cfg(any(feature = "std", feature = "libm"))]
#[test]
fn test_fast_norm_normal_values() {
let q = PQ64::new(1.1, 2.7, 3.4);
assert_eq!(q.fast_norm(), q.norm());
}
#[cfg(any(feature = "std", feature = "libm"))]
#[test]
fn test_fast_norm_zero_quaternion() {
let q = PQ32::zero();
assert_eq!(q.fast_norm(), 0.0);
}
#[cfg(any(feature = "std", feature = "libm"))]
#[test]
fn test_fast_norm_infinite_values() {
let inf = f32::INFINITY;
assert_eq!(PQ32::new(inf, 1.0, 1.0).fast_norm(), inf);
assert_eq!(PQ32::new(1.0, inf, 1.0).fast_norm(), inf);
assert_eq!(PQ32::new(1.0, 1.0, inf).fast_norm(), inf);
}
#[cfg(any(feature = "std", feature = "libm"))]
#[test]
fn test_fast_norm_nan_values() {
let nan = f32::NAN;
assert!(PQ32::new(nan, 1.0, 1.0).fast_norm().is_nan());
assert!(PQ32::new(1.0, nan, 1.0).fast_norm().is_nan());
assert!(PQ32::new(1.0, 1.0, nan).fast_norm().is_nan());
}
#[cfg(any(feature = "std", feature = "libm"))]
#[test]
fn test_exp_zero_quaternion() {
assert_eq!(PQ64::ZERO.exp(), UnitQuaternion::ONE);
}
#[cfg(any(feature = "std", feature = "libm"))]
#[test]
fn test_exp_i_quaternion() {
let q = PQ32::I;
let exp_q = q.exp();
let expected =
UnitQuaternion::new(1.0f32.cos(), 1.0f32.sin(), 0.0, 0.0);
assert_eq!(exp_q, expected);
}
#[cfg(any(feature = "std", feature = "libm"))]
#[test]
fn test_exp_complex_quaternion() {
let q = PQ64::new(1.0, 1.0, 1.0);
let exp_q = q.exp();
let angle = 3.0f64.sqrt();
let re = angle.cos();
let im = angle.sin() / angle;
let expected = UnitQuaternion::new(re, im, im, im);
assert!((exp_q - expected).norm() <= 2.0 * f64::EPSILON);
}
#[cfg(any(feature = "std", feature = "libm"))]
#[test]
fn test_exp_nan_quaternion() {
for q in [
PQ32::new(f32::NAN, 1.0, 1.0),
PQ32::new(1.0, f32::NAN, 1.0),
PQ32::new(1.0, 1.0, f32::NAN),
]
.iter()
{
let exp_q = q.exp();
assert!(exp_q.as_quaternion().w.is_nan());
assert!(exp_q.as_quaternion().x.is_nan());
assert!(exp_q.as_quaternion().y.is_nan());
assert!(exp_q.as_quaternion().z.is_nan());
}
}
#[cfg(any(feature = "std", feature = "libm"))]
#[test]
fn test_exp_large_imaginary_norm() {
let q = PQ32::new(1e30, 1e30, 1e30);
let exp_q = q.exp();
assert!(exp_q.as_quaternion().w.is_nan());
assert!(exp_q.as_quaternion().x.is_nan());
assert!(exp_q.as_quaternion().y.is_nan());
assert!(exp_q.as_quaternion().z.is_nan());
}
#[cfg(any(feature = "std", feature = "libm"))]
#[test]
fn test_exp_infinite_imaginary_part() {
let q = PQ64::new(1.0, 1.0, f64::INFINITY);
let exp_q = q.exp();
assert!(exp_q.as_quaternion().w.is_nan());
assert!(exp_q.as_quaternion().x.is_nan());
assert!(exp_q.as_quaternion().y.is_nan());
assert!(exp_q.as_quaternion().z.is_nan());
}
#[cfg(any(feature = "std", feature = "libm"))]
#[test]
fn test_exp_small_imaginary_norm() {
let epsilon = f32::EPSILON;
let q = PQ32::new(epsilon, epsilon, epsilon);
let exp_q = q.exp();
let expected = UnitQuaternion::new(1.0, epsilon, epsilon, epsilon);
assert!((exp_q - expected).norm() <= 0.5 * f32::EPSILON);
}
#[cfg(feature = "serde")]
#[test]
fn test_serde_pure_quaternion() {
let q = PQ32::new(1.0, 2.0, 3.0);
let serialized =
serde_json::to_string(&q).expect("Failed to serialize quaternion");
let deserialized: PQ32 = serde_json::from_str(&serialized)
.expect("Failed to deserialize quaternion");
assert_eq!(deserialized, q);
}
}