567 lines
15 KiB
Rust
567 lines
15 KiB
Rust
//! This module defines the [Quaternion][1] and
|
|
//! [DualQuaternion][2] structures.
|
|
//!
|
|
//! [1]: https://en.wikipedia.org/wiki/Quaternion
|
|
//! [2]: https://en.wikipedia.org/wiki/Dual_quaternion
|
|
use std::fmt::{Error, Formatter, Debug, Display};
|
|
use std::ops::{Add, Sub, Mul, Div, Neg};
|
|
|
|
use crate::zero::Zero;
|
|
use crate::one::One;
|
|
use crate::real::Real;
|
|
use crate::trig::Radian;
|
|
use crate::trig::Trig;
|
|
use crate::vector::{Vector, EuclideanVector, Vector3};
|
|
|
|
|
|
|
|
/// A Quaternion is a combination of a scalar and a vector
|
|
/// of complex numbers.
|
|
///
|
|
/// [S, V] = [S, Xi + Yj + Zk]
|
|
///
|
|
/// Remember that the complex number axes combine
|
|
/// in the following manner.
|
|
///
|
|
/// i^2 = j^2 = k^2 = -1
|
|
///
|
|
/// | | |
|
|
/// |:-------:|:-------:|
|
|
/// | ij = 1 | ik = -1 |
|
|
/// | jk = 1 | kj = -1 |
|
|
/// | ki = 1 | ji = -1 |
|
|
#[derive(Clone, Copy)]
|
|
pub struct Quaternion<T>
|
|
{
|
|
/// TODO: Describe this.
|
|
pub scalar: T,
|
|
|
|
/// TODO: Describe this.
|
|
pub vector: Vector3<T>
|
|
}
|
|
|
|
|
|
/// A macro that will define a binary operation
|
|
/// for a Quaternion and its components for
|
|
/// scalar values.
|
|
macro_rules! binary_operator_impl
|
|
{
|
|
($traitName: ident :: $funcName: ident, $structName: ident) =>
|
|
{
|
|
impl<T> $traitName<T> for $structName<T> where T: Trig
|
|
{
|
|
type Output = $structName<T>;
|
|
|
|
fn $funcName(self, scalar: T) -> $structName<T>
|
|
{
|
|
$structName::new(self.scalar.$funcName(scalar),
|
|
self.vector.$funcName(scalar))
|
|
}
|
|
}
|
|
|
|
impl<'a, T> $traitName<T> for &'a $structName<T> where T: Trig
|
|
{
|
|
type Output = $structName<T>;
|
|
|
|
fn $funcName(self, scalar: T) -> $structName<T>
|
|
{
|
|
$structName::new(self.scalar.$funcName(scalar),
|
|
self.vector.$funcName(scalar))
|
|
}
|
|
}
|
|
|
|
impl<'a, T> $traitName<&'a T> for $structName<T> where T: Trig
|
|
{
|
|
type Output = $structName<T>;
|
|
|
|
fn $funcName(self, scalar: &'a T) -> $structName<T>
|
|
{
|
|
$structName::new(self.scalar.$funcName(*scalar),
|
|
self.vector.$funcName(*scalar))
|
|
}
|
|
}
|
|
|
|
impl<'a, 'b, T> $traitName<&'b T> for &'a $structName<T> where T: Trig
|
|
{
|
|
type Output = $structName<T>;
|
|
|
|
fn $funcName(self, scalar: &'b T) -> $structName<T>
|
|
{
|
|
$structName::new(self.scalar.$funcName(*scalar),
|
|
self.vector.$funcName(*scalar))
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
// Implement the Quaternion's methods.
|
|
impl<T> Quaternion<T> where T: Trig
|
|
{
|
|
/// Create a new Quaternion from a given scalar and Vector.
|
|
///
|
|
///```
|
|
/// // Create a new Quaternion<f64> where all
|
|
/// // the components are 5.5f64.
|
|
/// use sigils::vector::*;
|
|
/// use sigils::quaternion::*;
|
|
///
|
|
/// let quaternion =
|
|
/// Quaternion::<f64>::new(5.5f64, Vector3::new(5.5f64, 5.5f64, 5.5f64));
|
|
/// # assert_eq!(quaternion.scalar, 5.5f64);
|
|
/// # assert_eq!(quaternion.vector.x, 5.5f64);
|
|
/// # assert_eq!(quaternion.vector.y, 5.5f64);
|
|
/// # assert_eq!(quaternion.vector.z, 5.5f64);
|
|
///```
|
|
pub fn new(s: T, v: Vector3<T>) -> Quaternion<T>
|
|
{
|
|
Quaternion {scalar: s, vector: v}
|
|
}
|
|
|
|
/// Create a new Quaternion from the given values.
|
|
///
|
|
///```
|
|
/// // Create a new Quaternion<f32> where all
|
|
/// // the components are 5.5f32.
|
|
/// use sigils::quaternion::*;
|
|
///
|
|
/// let quaternion =
|
|
/// Quaternion::<f32>::from_values(5.5f32, 5.5f32, 5.5f32, 5.5f32);
|
|
/// # assert_eq!(quaternion.scalar, 5.5f32);
|
|
/// # assert_eq!(quaternion.vector.x, 5.5f32);
|
|
/// # assert_eq!(quaternion.vector.y, 5.5f32);
|
|
/// # assert_eq!(quaternion.vector.z, 5.5f32);
|
|
///```
|
|
pub fn from_values(s: T, xi: T, yj: T, zk: T) -> Quaternion<T>
|
|
{
|
|
Quaternion::new(s, Vector3::new(xi, yj, zk))
|
|
}
|
|
|
|
/// Create an identity Quaternion. This is a Quaternion
|
|
/// with a zero Vector and a scalar of one.
|
|
///
|
|
///```
|
|
/// // Create a new identity Quaternion<f32>.
|
|
/// use sigils::quaternion::*;
|
|
///
|
|
/// let quaternion =
|
|
/// Quaternion::<f32>::identity();
|
|
/// # assert_eq!(quaternion.scalar, 1.0f32);
|
|
/// # assert_eq!(quaternion.vector.x, 0.0f32);
|
|
/// # assert_eq!(quaternion.vector.y, 0.0f32);
|
|
/// # assert_eq!(quaternion.vector.z, 0.0f32);
|
|
///```
|
|
pub fn identity() -> Quaternion<T>
|
|
{
|
|
Quaternion::new(T::one(), Vector3::zero())
|
|
}
|
|
|
|
/// Get the squared magnitude of the Quaternion.
|
|
/// This is basically a length equation.
|
|
///
|
|
/// S^2 + V^2
|
|
///
|
|
///```
|
|
/// // Create a new Quaternion<f64> where all
|
|
/// // the components are 5.5f64 and get its
|
|
/// // squared magnitude.
|
|
/// use sigils::vector::*;
|
|
/// use sigils::quaternion::*;
|
|
///
|
|
/// let quaternion =
|
|
/// Quaternion::<f64>::from_values(5.5f64, 5.5f64, 5.5f64, 5.5f64);
|
|
/// let magnitude = quaternion.get_magnitude_squared();
|
|
/// # assert_eq!(magnitude, 121.0f64);
|
|
///```
|
|
pub fn get_magnitude_squared(&self) -> T
|
|
{
|
|
(self.scalar * self.scalar) + self.vector.get_length_squared()
|
|
}
|
|
|
|
/// Get the magnitude of the Quaternion.
|
|
/// This is basically a length equation.
|
|
///
|
|
///```
|
|
/// // Create a new Quaternion<f32> where all
|
|
/// // the components are 5.5f32 and get its
|
|
/// // magnitude.
|
|
/// use sigils::quaternion::*;
|
|
///
|
|
/// let quaternion =
|
|
/// Quaternion::<f32>::from_values(5.5f32, 5.5f32, 5.5f32, 5.5f32);
|
|
/// let magnitude = quaternion.get_magnitude();
|
|
/// # assert_eq!(magnitude, 11.0f32);
|
|
///```
|
|
pub fn get_magnitude(&self) -> T
|
|
{
|
|
let magnitude: T;
|
|
|
|
// Calculate the magnitude for the Quaternion
|
|
// and make sure the magnitude is actually
|
|
// a number.
|
|
magnitude = self.get_magnitude_squared().sqrt();
|
|
assert!(magnitude.is_finite());
|
|
|
|
// Return the calculated magnitude.
|
|
magnitude
|
|
}
|
|
|
|
/// Set this Quaternion to the identity Quaternion.
|
|
/// This is a Quaternion with a zero Vector and
|
|
/// a scalar of one.
|
|
pub fn set_identity(&mut self)
|
|
{
|
|
self.scalar = T::one();
|
|
self.vector = Vector3::zero();
|
|
}
|
|
|
|
/// Negates only the Vector component.
|
|
pub fn conjugate_self(&mut self)
|
|
{
|
|
self.vector = -self.vector;
|
|
}
|
|
|
|
/// Normalizes this Quaternions values.
|
|
///
|
|
/// [S/magnitude, V/magnitude]
|
|
pub fn normalize_self(&mut self)
|
|
{
|
|
let inverse_mag: T;
|
|
|
|
// Get the inverse of the magnitude.
|
|
inverse_mag = T::one() / self.get_magnitude();
|
|
|
|
// Multily the scalar and Vector components
|
|
// by the inverse magnitude.
|
|
self.scalar = self.scalar * inverse_mag;
|
|
self.vector = self.vector * inverse_mag;
|
|
}
|
|
|
|
/// Invert this Quaternion.
|
|
pub fn invert_self(&mut self)
|
|
{
|
|
// Conjugate this Quaternion
|
|
// and then normalize it.
|
|
self.conjugate_self();
|
|
self.normalize_self();
|
|
}
|
|
|
|
/// Create a new Quaternion that is the same
|
|
/// as this Quaternion with a negated Vector component.
|
|
pub fn conjugate(&self) -> Quaternion<T>
|
|
{
|
|
// Create a new Quaternion that has
|
|
// the same scalar component value and
|
|
// a negated version of the Vector component.
|
|
Quaternion::new(self.scalar.clone(), -self.vector.clone())
|
|
}
|
|
|
|
/// Create a new Quaternion that is a copy of this
|
|
/// Quaternion with normalized values.
|
|
///
|
|
/// [S/magnitude, V/magnitude]
|
|
pub fn normalize(&self) -> Quaternion<T>
|
|
{
|
|
let inverse_mag: T;
|
|
|
|
inverse_mag = T::one() / self.get_magnitude();
|
|
|
|
self * inverse_mag
|
|
}
|
|
|
|
/// Create a new Quaternion that is a normalized
|
|
/// conjugate of this Quaternion.
|
|
pub fn invert(&self) -> Quaternion<T>
|
|
{
|
|
self.conjugate().normalize()
|
|
}
|
|
|
|
// TODO: Add binary operations for editing self.
|
|
}
|
|
|
|
// Implement the Debug trait for the Quaternion.
|
|
impl<T> Debug for Quaternion<T> where T: Trig
|
|
{
|
|
fn fmt(&self, formatter: &mut Formatter) -> Result<(), Error>
|
|
{
|
|
write!(formatter, "[{:?}, {:?}]", self.scalar, self.vector)
|
|
}
|
|
}
|
|
|
|
// Implement the Display trait for the Quaternion.
|
|
impl<T> Display for Quaternion<T> where T: Trig
|
|
{
|
|
fn fmt(&self, formatter: &mut Formatter) -> Result<(), Error>
|
|
{
|
|
write!(formatter, "[{}, {}]", self.scalar, self.vector)
|
|
}
|
|
}
|
|
|
|
// Implement a default value for Quaternions which
|
|
// returns the identity quaternion.
|
|
impl<T> Default for Quaternion<T> where T: Trig
|
|
{
|
|
fn default() -> Quaternion<T>
|
|
{
|
|
Quaternion::identity()
|
|
}
|
|
}
|
|
|
|
// Implement the Zero and One traits for the Quaternion.
|
|
impl<T> Zero for Quaternion<T> where T: Trig
|
|
{
|
|
fn zero() -> Quaternion<T>
|
|
{
|
|
Quaternion::new(T::zero(), Vector3::zero())
|
|
}
|
|
}
|
|
|
|
impl<T> One for Quaternion<T> where T: Trig
|
|
{
|
|
fn one() -> Quaternion<T>
|
|
{
|
|
Quaternion::identity()
|
|
}
|
|
}
|
|
|
|
// Implement the binary operations for Quaternions and scalars.
|
|
binary_operator_impl!(Mul::mul, Quaternion);
|
|
//binary_operator_impl!(Div::div, Quaternion);
|
|
//binary_operator_impl!(Neg::neg, Quaternion);
|
|
|
|
// Implement Negating a Quaternion.
|
|
impl<T> Neg for Quaternion<T> where T: Trig
|
|
{
|
|
type Output = Quaternion<T>;
|
|
|
|
fn neg(self) -> Quaternion<T>
|
|
{
|
|
Quaternion::new(-self.scalar, -self.vector)
|
|
}
|
|
}
|
|
|
|
impl<'a, T> Neg for &'a Quaternion<T> where T: Trig
|
|
{
|
|
type Output = Quaternion<T>;
|
|
|
|
fn neg(self) -> Quaternion<T>
|
|
{
|
|
Quaternion::new(-self.scalar, -self.vector)
|
|
}
|
|
}
|
|
|
|
// Implement Adding Quaternions.
|
|
impl<T> Add<Quaternion<T>> for Quaternion<T> where T: Trig
|
|
{
|
|
type Output = Quaternion<T>;
|
|
|
|
fn add(self, _rhs: Quaternion<T>) -> Quaternion<T>
|
|
{
|
|
Quaternion::new(self.scalar + _rhs.scalar,
|
|
self.vector + _rhs.vector)
|
|
}
|
|
}
|
|
|
|
impl<'a, T> Add<Quaternion<T>> for &'a Quaternion<T> where T: Trig
|
|
{
|
|
type Output = Quaternion<T>;
|
|
|
|
fn add(self, _rhs: Quaternion<T>) -> Quaternion<T>
|
|
{
|
|
Quaternion::new(self.scalar + _rhs.scalar,
|
|
self.vector + _rhs.vector)
|
|
}
|
|
}
|
|
|
|
impl<'a, T> Add<&'a Quaternion<T>> for Quaternion<T> where T: Trig
|
|
{
|
|
type Output = Quaternion<T>;
|
|
|
|
fn add(self, _rhs: &'a Quaternion<T>) -> Quaternion<T>
|
|
{
|
|
Quaternion::new(self.scalar + _rhs.scalar,
|
|
self.vector + _rhs.vector)
|
|
}
|
|
}
|
|
|
|
impl<'a, 'b, T> Add<&'b Quaternion<T>> for &'a Quaternion<T> where T: Trig
|
|
{
|
|
type Output = Quaternion<T>;
|
|
|
|
fn add(self, _rhs: &'b Quaternion<T>) -> Quaternion<T>
|
|
{
|
|
Quaternion::new(self.scalar + _rhs.scalar,
|
|
self.vector + _rhs.vector)
|
|
}
|
|
}
|
|
|
|
// Implement subtracting Quaternions.
|
|
impl<T> Sub<Quaternion<T>> for Quaternion<T> where T: Trig
|
|
{
|
|
type Output = Quaternion<T>;
|
|
|
|
fn sub(self, _rhs: Quaternion<T>) -> Quaternion<T>
|
|
{
|
|
Quaternion::new(self.scalar - _rhs.scalar,
|
|
self.vector - _rhs.vector)
|
|
}
|
|
}
|
|
|
|
impl<'a, T> Sub<Quaternion<T>> for &'a Quaternion<T> where T: Trig
|
|
{
|
|
type Output = Quaternion<T>;
|
|
|
|
fn sub(self, _rhs: Quaternion<T>) -> Quaternion<T>
|
|
{
|
|
Quaternion::new(self.scalar - _rhs.scalar,
|
|
self.vector - _rhs.vector)
|
|
}
|
|
}
|
|
|
|
impl<'a, T> Sub<&'a Quaternion<T>> for Quaternion<T> where T: Trig
|
|
{
|
|
type Output = Quaternion<T>;
|
|
|
|
fn sub(self, _rhs: &'a Quaternion<T>) -> Quaternion<T>
|
|
{
|
|
Quaternion::new(self.scalar - _rhs.scalar,
|
|
self.vector - _rhs.vector)
|
|
}
|
|
}
|
|
|
|
impl<'a, 'b, T> Sub<&'b Quaternion<T>> for &'a Quaternion<T> where T: Trig
|
|
{
|
|
type Output = Quaternion<T>;
|
|
|
|
fn sub(self, _rhs: &'b Quaternion<T>) -> Quaternion<T>
|
|
{
|
|
Quaternion::new(self.scalar - _rhs.scalar,
|
|
self.vector - _rhs.vector)
|
|
}
|
|
}
|
|
|
|
// Implement Dividing Quaternions.
|
|
impl<T> Div<Quaternion<T>> for Quaternion<T> where T: Trig
|
|
{
|
|
type Output = Quaternion<T>;
|
|
|
|
fn div(self, _rhs: Quaternion<T>) -> Quaternion<T>
|
|
{
|
|
self * _rhs.invert()
|
|
}
|
|
}
|
|
|
|
impl<'a, T> Div<Quaternion<T>> for &'a Quaternion<T> where T: Trig
|
|
{
|
|
type Output = Quaternion<T>;
|
|
|
|
fn div(self, _rhs: Quaternion<T>) -> Quaternion<T>
|
|
{
|
|
self * _rhs.invert()
|
|
}
|
|
}
|
|
|
|
impl<'a, T> Div<&'a Quaternion<T>> for Quaternion<T> where T: Trig
|
|
{
|
|
type Output = Quaternion<T>;
|
|
|
|
fn div(self, _rhs: &'a Quaternion<T>) -> Quaternion<T>
|
|
{
|
|
self * _rhs.invert()
|
|
}
|
|
}
|
|
|
|
impl<'a, 'b, T> Div<&'b Quaternion<T>> for &'a Quaternion<T> where T: Trig
|
|
{
|
|
type Output = Quaternion<T>;
|
|
|
|
fn div(self, _rhs: &'b Quaternion<T>) -> Quaternion<T>
|
|
{
|
|
self * _rhs.invert()
|
|
}
|
|
}
|
|
|
|
// Implement multiplying Quaternions and Vectors.
|
|
impl<T> Mul<Quaternion<T>> for Quaternion<T> where T: Trig
|
|
{
|
|
type Output = Quaternion<T>;
|
|
|
|
fn mul(self, _rhs: Quaternion<T>) -> Quaternion<T>
|
|
{
|
|
multiply_quaternions(self.scalar, self.vector.x,
|
|
self.vector.y, self.vector.z,
|
|
_rhs.scalar, _rhs.vector.x,
|
|
_rhs.vector.y, _rhs.vector.z)
|
|
}
|
|
}
|
|
|
|
impl<'a, T> Mul<Quaternion<T>> for &'a Quaternion<T> where T: Trig
|
|
{
|
|
type Output = Quaternion<T>;
|
|
|
|
fn mul(self, _rhs: Quaternion<T>) -> Quaternion<T>
|
|
{
|
|
multiply_quaternions(self.scalar, self.vector.x,
|
|
self.vector.y, self.vector.z,
|
|
_rhs.scalar, _rhs.vector.x,
|
|
_rhs.vector.y, _rhs.vector.z)
|
|
}
|
|
}
|
|
|
|
impl<'a, T> Mul<&'a Quaternion<T>> for Quaternion<T> where T: Trig
|
|
{
|
|
type Output = Quaternion<T>;
|
|
|
|
fn mul(self, _rhs: &'a Quaternion<T>) -> Quaternion<T>
|
|
{
|
|
multiply_quaternions(self.scalar, self.vector.x,
|
|
self.vector.y, self.vector.z,
|
|
_rhs.scalar, _rhs.vector.x,
|
|
_rhs.vector.y, _rhs.vector.z)
|
|
}
|
|
}
|
|
|
|
impl<'a, 'b, T> Mul<&'b Quaternion<T>> for &'a Quaternion<T> where T: Trig
|
|
{
|
|
type Output = Quaternion<T>;
|
|
|
|
fn mul(self, _rhs: &'b Quaternion<T>) -> Quaternion<T>
|
|
{
|
|
multiply_quaternions(self.scalar, self.vector.x,
|
|
self.vector.y, self.vector.z,
|
|
_rhs.scalar, _rhs.vector.x,
|
|
_rhs.vector.y, _rhs.vector.z)
|
|
}
|
|
}
|
|
|
|
|
|
|
|
/// This is a private function for multiplying the
|
|
/// values of two Quaternions. This was created to
|
|
/// follow the DRY principle.
|
|
///
|
|
/// Qa = [Sa, A] = [Sa, Xai + Yaj + Zak]
|
|
/// Qb = [Sb, B] = [Sb, Xbi + Ybj + Zbk]
|
|
///
|
|
/// QaQb = [Sa, A][Sb, B] = [Sa, Xai + Yaj + Zak][Sb, Xbi + Ybj + Zbk]
|
|
/// QaQb = (SaSb - XaXb - YaYb - ZaZb) +
|
|
/// (SaXb + XaSb + YaZb - ZaYb)i +
|
|
/// (SaYb - XaZb + SbYa + ZaXb)j +
|
|
/// (SaZb + XaYb - YaXb + SbZa)k
|
|
fn multiply_quaternions<T>(sa: T, xa: T, ya: T, za: T,
|
|
sb: T, xb: T, yb: T, zb: T)
|
|
-> Quaternion<T> where T: Trig
|
|
{
|
|
let i: T;
|
|
let j: T;
|
|
let k: T;
|
|
let scalar: T;
|
|
|
|
i = (sa * xb) + (xa * sb) + (ya * zb) - (za * yb);
|
|
j = (sa * yb) - (xa * zb) + (ya * sb) + (za * xb);
|
|
k = (sa * zb) + (xa * yb) - (ya * xb) + (za * sb);
|
|
scalar = (sa * sb) - (xa * xb) - (ya * yb) - (za * zb);
|
|
|
|
Quaternion::new(scalar, Vector3::new(i, j, k))
|
|
}
|