2016-12-05 05:44:42 +08:00
|
|
|
use num::Zero;
|
|
|
|
|
2018-10-22 13:00:10 +08:00
|
|
|
use alga::general::{
|
|
|
|
AbstractGroup, AbstractGroupAbelian, AbstractLoop, AbstractMagma, AbstractModule,
|
|
|
|
AbstractMonoid, AbstractQuasigroup, AbstractSemigroup, Additive, Id, Identity, Inverse, Module,
|
|
|
|
Multiplicative, Real,
|
|
|
|
};
|
|
|
|
use alga::linear::{
|
|
|
|
AffineTransformation, DirectIsometry, FiniteDimVectorSpace, Isometry, NormedSpace,
|
|
|
|
OrthogonalTransformation, ProjectiveTransformation, Rotation, Similarity, Transformation,
|
|
|
|
VectorSpace,
|
|
|
|
};
|
2016-12-05 05:44:42 +08:00
|
|
|
|
2018-05-19 23:15:15 +08:00
|
|
|
use base::{Vector3, Vector4};
|
2017-08-03 01:37:44 +08:00
|
|
|
use geometry::{Point3, Quaternion, UnitQuaternion};
|
2016-12-05 05:44:42 +08:00
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
impl<N: Real> Identity<Multiplicative> for Quaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
#[inline]
|
|
|
|
fn identity() -> Self {
|
|
|
|
Self::identity()
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
impl<N: Real> Identity<Additive> for Quaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
#[inline]
|
|
|
|
fn identity() -> Self {
|
|
|
|
Self::zero()
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
impl<N: Real> AbstractMagma<Multiplicative> for Quaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
#[inline]
|
|
|
|
fn operate(&self, rhs: &Self) -> Self {
|
|
|
|
self * rhs
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
impl<N: Real> AbstractMagma<Additive> for Quaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
#[inline]
|
|
|
|
fn operate(&self, rhs: &Self) -> Self {
|
|
|
|
self + rhs
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
impl<N: Real> Inverse<Additive> for Quaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
#[inline]
|
|
|
|
fn inverse(&self) -> Self {
|
|
|
|
-self
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
macro_rules! impl_structures(
|
|
|
|
($Quaternion: ident; $($marker: ident<$operator: ident>),* $(,)*) => {$(
|
2017-08-03 01:37:44 +08:00
|
|
|
impl<N: Real> $marker<$operator> for $Quaternion<N> { }
|
2016-12-05 05:44:42 +08:00
|
|
|
)*}
|
|
|
|
);
|
|
|
|
|
|
|
|
impl_structures!(
|
2017-08-03 01:37:44 +08:00
|
|
|
Quaternion;
|
2016-12-05 05:44:42 +08:00
|
|
|
AbstractSemigroup<Multiplicative>,
|
|
|
|
AbstractMonoid<Multiplicative>,
|
|
|
|
|
|
|
|
AbstractSemigroup<Additive>,
|
|
|
|
AbstractQuasigroup<Additive>,
|
|
|
|
AbstractMonoid<Additive>,
|
|
|
|
AbstractLoop<Additive>,
|
|
|
|
AbstractGroup<Additive>,
|
|
|
|
AbstractGroupAbelian<Additive>
|
|
|
|
);
|
|
|
|
|
|
|
|
/*
|
|
|
|
*
|
|
|
|
* Vector space.
|
|
|
|
*
|
|
|
|
*/
|
2017-08-03 01:37:44 +08:00
|
|
|
impl<N: Real> AbstractModule for Quaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
type AbstractRing = N;
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
fn multiply_by(&self, n: N) -> Self {
|
|
|
|
self * n
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
impl<N: Real> Module for Quaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
type Ring = N;
|
|
|
|
}
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
impl<N: Real> VectorSpace for Quaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
type Field = N;
|
|
|
|
}
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
impl<N: Real> FiniteDimVectorSpace for Quaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
#[inline]
|
|
|
|
fn dimension() -> usize {
|
|
|
|
4
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
fn canonical_basis_element(i: usize) -> Self {
|
2018-10-28 14:33:39 +08:00
|
|
|
Self::from(Vector4::canonical_basis_element(i))
|
2016-12-05 05:44:42 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
fn dot(&self, other: &Self) -> N {
|
|
|
|
self.coords.dot(&other.coords)
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
unsafe fn component_unchecked(&self, i: usize) -> &N {
|
|
|
|
self.coords.component_unchecked(i)
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
unsafe fn component_unchecked_mut(&mut self, i: usize) -> &mut N {
|
|
|
|
self.coords.component_unchecked_mut(i)
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
impl<N: Real> NormedSpace for Quaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
#[inline]
|
|
|
|
fn norm_squared(&self) -> N {
|
|
|
|
self.coords.norm_squared()
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
fn norm(&self) -> N {
|
|
|
|
self.as_vector().norm()
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
fn normalize(&self) -> Self {
|
|
|
|
let v = self.coords.normalize();
|
2018-10-28 14:33:39 +08:00
|
|
|
Self::from(v)
|
2016-12-05 05:44:42 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
fn normalize_mut(&mut self) -> N {
|
|
|
|
self.coords.normalize_mut()
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
fn try_normalize(&self, min_norm: N) -> Option<Self> {
|
|
|
|
if let Some(v) = self.coords.try_normalize(min_norm) {
|
2018-10-28 14:33:39 +08:00
|
|
|
Some(Self::from(v))
|
2018-02-02 19:26:35 +08:00
|
|
|
} else {
|
2016-12-05 05:44:42 +08:00
|
|
|
None
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
fn try_normalize_mut(&mut self, min_norm: N) -> Option<N> {
|
|
|
|
self.coords.try_normalize_mut(min_norm)
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
*
|
2017-08-03 01:37:44 +08:00
|
|
|
* Implementations for UnitQuaternion.
|
2016-12-05 05:44:42 +08:00
|
|
|
*
|
|
|
|
*/
|
2017-08-03 01:37:44 +08:00
|
|
|
impl<N: Real> Identity<Multiplicative> for UnitQuaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
#[inline]
|
|
|
|
fn identity() -> Self {
|
|
|
|
Self::identity()
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
impl<N: Real> AbstractMagma<Multiplicative> for UnitQuaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
#[inline]
|
|
|
|
fn operate(&self, rhs: &Self) -> Self {
|
|
|
|
self * rhs
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
impl<N: Real> Inverse<Multiplicative> for UnitQuaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
#[inline]
|
|
|
|
fn inverse(&self) -> Self {
|
|
|
|
self.inverse()
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
fn inverse_mut(&mut self) {
|
|
|
|
self.inverse_mut()
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
impl_structures!(
|
2017-08-03 01:37:44 +08:00
|
|
|
UnitQuaternion;
|
2016-12-05 05:44:42 +08:00
|
|
|
AbstractSemigroup<Multiplicative>,
|
|
|
|
AbstractQuasigroup<Multiplicative>,
|
|
|
|
AbstractMonoid<Multiplicative>,
|
|
|
|
AbstractLoop<Multiplicative>,
|
|
|
|
AbstractGroup<Multiplicative>
|
|
|
|
);
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
impl<N: Real> Transformation<Point3<N>> for UnitQuaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
fn transform_point(&self, pt: &Point3<N>) -> Point3<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
self * pt
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
fn transform_vector(&self, v: &Vector3<N>) -> Vector3<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
self * v
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
impl<N: Real> ProjectiveTransformation<Point3<N>> for UnitQuaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
fn inverse_transform_point(&self, pt: &Point3<N>) -> Point3<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
// FIXME: would it be useful performancewise not to call inverse explicitly (i-e. implement
|
|
|
|
// the inverse transformation explicitly here) ?
|
|
|
|
self.inverse() * pt
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
fn inverse_transform_vector(&self, v: &Vector3<N>) -> Vector3<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
self.inverse() * v
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
impl<N: Real> AffineTransformation<Point3<N>> for UnitQuaternion<N> {
|
2018-02-02 19:26:35 +08:00
|
|
|
type Rotation = Self;
|
2016-12-05 05:44:42 +08:00
|
|
|
type NonUniformScaling = Id;
|
2018-02-02 19:26:35 +08:00
|
|
|
type Translation = Id;
|
2016-12-05 05:44:42 +08:00
|
|
|
|
|
|
|
#[inline]
|
|
|
|
fn decompose(&self) -> (Id, Self, Id, Self) {
|
|
|
|
(Id::new(), self.clone(), Id::new(), Self::identity())
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
fn append_translation(&self, _: &Self::Translation) -> Self {
|
|
|
|
self.clone()
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
fn prepend_translation(&self, _: &Self::Translation) -> Self {
|
|
|
|
self.clone()
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
fn append_rotation(&self, r: &Self::Rotation) -> Self {
|
|
|
|
r * self
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
fn prepend_rotation(&self, r: &Self::Rotation) -> Self {
|
|
|
|
self * r
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
fn append_scaling(&self, _: &Self::NonUniformScaling) -> Self {
|
|
|
|
self.clone()
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
fn prepend_scaling(&self, _: &Self::NonUniformScaling) -> Self {
|
|
|
|
self.clone()
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
impl<N: Real> Similarity<Point3<N>> for UnitQuaternion<N> {
|
2018-02-02 19:26:35 +08:00
|
|
|
type Scaling = Id;
|
2016-12-05 05:44:42 +08:00
|
|
|
|
|
|
|
#[inline]
|
|
|
|
fn translation(&self) -> Id {
|
|
|
|
Id::new()
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
fn rotation(&self) -> Self {
|
|
|
|
self.clone()
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline]
|
|
|
|
fn scaling(&self) -> Id {
|
|
|
|
Id::new()
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
macro_rules! marker_impl(
|
|
|
|
($($Trait: ident),*) => {$(
|
2017-08-03 01:37:44 +08:00
|
|
|
impl<N: Real> $Trait<Point3<N>> for UnitQuaternion<N> { }
|
2016-12-05 05:44:42 +08:00
|
|
|
)*}
|
|
|
|
);
|
|
|
|
|
|
|
|
marker_impl!(Isometry, DirectIsometry, OrthogonalTransformation);
|
|
|
|
|
2017-08-03 01:37:44 +08:00
|
|
|
impl<N: Real> Rotation<Point3<N>> for UnitQuaternion<N> {
|
2016-12-05 05:44:42 +08:00
|
|
|
#[inline]
|
|
|
|
fn powf(&self, n: N) -> Option<Self> {
|
|
|
|
Some(self.powf(n))
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
fn rotation_between(a: &Vector3<N>, b: &Vector3<N>) -> Option<Self> {
|
2016-12-05 05:44:42 +08:00
|
|
|
Self::rotation_between(a, b)
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline]
|
2017-08-03 01:37:44 +08:00
|
|
|
fn scaled_rotation_between(a: &Vector3<N>, b: &Vector3<N>, s: N) -> Option<Self> {
|
2016-12-05 05:44:42 +08:00
|
|
|
Self::scaled_rotation_between(a, b, s)
|
|
|
|
}
|
|
|
|
}
|