545 lines
14 KiB
Rust
545 lines
14 KiB
Rust
//! Quaternion definition.
|
||
|
||
#![allow(missing_docs)] // we allow missing to avoid having to document the dispatch trait.
|
||
|
||
use std::mem;
|
||
use std::slice::{Iter, IterMut};
|
||
use std::ops::{Add, Sub, Mul, Div, Neg, Index, IndexMut};
|
||
use std::iter::{FromIterator, IntoIterator};
|
||
use rand::{Rand, Rng};
|
||
use num::{Zero, One};
|
||
use structs::{Vec3, Pnt3, Rot3, Mat3};
|
||
use traits::operations::{ApproxEq, Inv, POrd, POrdering, Axpy};
|
||
use traits::structure::{Cast, Indexable, Iterable, IterableMut, Dim, Shape, BaseFloat, BaseNum,
|
||
Bounded, Repeat};
|
||
use traits::geometry::{Norm, Rotation, RotationMatrix, Rotate, RotationTo, Transform};
|
||
|
||
#[cfg(feature="arbitrary")]
|
||
use quickcheck::{Arbitrary, Gen};
|
||
|
||
|
||
/// A quaternion.
|
||
#[repr(C)]
|
||
#[derive(Eq, PartialEq, RustcEncodable, RustcDecodable, Clone, Hash, Debug, Copy)]
|
||
pub struct Quat<N> {
|
||
/// The scalar component of the quaternion.
|
||
pub w: N,
|
||
/// The first vector component of the quaternion.
|
||
pub i: N,
|
||
/// The second vector component of the quaternion.
|
||
pub j: N,
|
||
/// The third vector component of the quaternion.
|
||
pub k: N
|
||
}
|
||
|
||
impl<N> Quat<N> {
|
||
/// Creates a new quaternion from its components.
|
||
#[inline]
|
||
pub fn new(w: N, i: N, j: N, k: N) -> Quat<N> {
|
||
Quat {
|
||
w: w,
|
||
i: i,
|
||
j: j,
|
||
k: k
|
||
}
|
||
}
|
||
|
||
/// The vector part `(i, j, k)` of this quaternion.
|
||
#[inline]
|
||
pub fn vector<'a>(&'a self) -> &'a Vec3<N> {
|
||
// FIXME: do this require a `repr(C)` ?
|
||
unsafe {
|
||
mem::transmute(&self.i)
|
||
}
|
||
}
|
||
|
||
/// The scalar part `w` of this quaternion.
|
||
#[inline]
|
||
pub fn scalar<'a>(&'a self) -> &'a N {
|
||
&self.w
|
||
}
|
||
}
|
||
|
||
impl<N: Neg<Output = N> + Copy> Quat<N> {
|
||
/// Compute the conjugate of this quaternion.
|
||
#[inline]
|
||
pub fn conjugate(&self) -> Quat<N> {
|
||
Quat { w: self.w, i: -self.i, j: -self.j, k: -self.k }
|
||
}
|
||
|
||
/// Replaces this quaternion by its conjugate.
|
||
#[inline]
|
||
pub fn conjugate_mut(&mut self) {
|
||
self.i = -self.i;
|
||
self.j = -self.j;
|
||
self.k = -self.k;
|
||
}
|
||
}
|
||
|
||
impl<N: BaseFloat + ApproxEq<N>> Inv for Quat<N> {
|
||
#[inline]
|
||
fn inv(&self) -> Option<Quat<N>> {
|
||
let mut res = *self;
|
||
|
||
if res.inv_mut() {
|
||
Some(res)
|
||
}
|
||
else {
|
||
None
|
||
}
|
||
}
|
||
|
||
#[inline]
|
||
fn inv_mut(&mut self) -> bool {
|
||
let sqnorm = Norm::sqnorm(self);
|
||
|
||
if ApproxEq::approx_eq(&sqnorm, &::zero()) {
|
||
false
|
||
}
|
||
else {
|
||
self.conjugate_mut();
|
||
self.w = self.w / sqnorm;
|
||
self.i = self.i / sqnorm;
|
||
self.j = self.j / sqnorm;
|
||
self.k = self.k / sqnorm;
|
||
|
||
true
|
||
}
|
||
}
|
||
}
|
||
|
||
impl<N: BaseFloat> Norm<N> for Quat<N> {
|
||
#[inline]
|
||
fn sqnorm(&self) -> N {
|
||
self.w * self.w + self.i * self.i + self.j * self.j + self.k * self.k
|
||
}
|
||
|
||
#[inline]
|
||
fn normalize(&self) -> Quat<N> {
|
||
let n = self.norm();
|
||
Quat::new(self.w / n, self.i / n, self.j / n, self.k / n)
|
||
}
|
||
|
||
#[inline]
|
||
fn normalize_mut(&mut self) -> N {
|
||
let n = Norm::norm(self);
|
||
|
||
self.w = self.w / n;
|
||
self.i = self.i / n;
|
||
self.j = self.j / n;
|
||
self.k = self.k / n;
|
||
|
||
n
|
||
}
|
||
}
|
||
|
||
impl<N> Mul<Quat<N>> for Quat<N>
|
||
where N: Copy + Mul<N, Output = N> + Sub<N, Output = N> + Add<N, Output = N> {
|
||
type Output = Quat<N>;
|
||
|
||
#[inline]
|
||
fn mul(self, right: Quat<N>) -> Quat<N> {
|
||
Quat::new(
|
||
self.w * right.w - self.i * right.i - self.j * right.j - self.k * right.k,
|
||
self.w * right.i + self.i * right.w + self.j * right.k - self.k * right.j,
|
||
self.w * right.j - self.i * right.k + self.j * right.w + self.k * right.i,
|
||
self.w * right.k + self.i * right.j - self.j * right.i + self.k * right.w)
|
||
}
|
||
}
|
||
|
||
impl<N: ApproxEq<N> + BaseFloat> Div<Quat<N>> for Quat<N> {
|
||
type Output = Quat<N>;
|
||
|
||
#[inline]
|
||
fn div(self, right: Quat<N>) -> Quat<N> {
|
||
self * right.inv().expect("Unable to invert the denominator.")
|
||
}
|
||
}
|
||
|
||
rand_impl!(Quat, w, i, j, k);
|
||
|
||
|
||
/// A unit quaternion that can represent a 3D rotation.
|
||
#[repr(C)]
|
||
#[derive(Eq, PartialEq, RustcEncodable, RustcDecodable, Clone, Hash, Debug, Copy)]
|
||
pub struct UnitQuat<N> {
|
||
q: Quat<N>
|
||
}
|
||
|
||
impl<N: BaseFloat> UnitQuat<N> {
|
||
/// Creates a new unit quaternion from the axis-angle representation of a rotation.
|
||
#[inline]
|
||
pub fn new(axisangle: Vec3<N>) -> UnitQuat<N> {
|
||
let sqang = Norm::sqnorm(&axisangle);
|
||
|
||
if ::is_zero(&sqang) {
|
||
::one()
|
||
}
|
||
else {
|
||
let ang = sqang.sqrt();
|
||
let (s, c) = (ang / Cast::from(2.0)).sin_cos();
|
||
|
||
let s_ang = s / ang;
|
||
|
||
unsafe {
|
||
UnitQuat::new_with_unit_quat(
|
||
Quat::new(
|
||
c,
|
||
axisangle.x * s_ang,
|
||
axisangle.y * s_ang,
|
||
axisangle.z * s_ang)
|
||
)
|
||
}
|
||
}
|
||
}
|
||
|
||
/// Creates a new unit quaternion from a quaternion.
|
||
///
|
||
/// The input quaternion will be normalized.
|
||
#[inline]
|
||
pub fn new_with_quat(q: Quat<N>) -> UnitQuat<N> {
|
||
UnitQuat { q: q.normalize() }
|
||
}
|
||
|
||
/// Creates a new unit quaternion from Euler angles.
|
||
///
|
||
/// The primitive rotations are applied in order: 1 roll − 2 pitch − 3 yaw.
|
||
#[inline]
|
||
pub fn new_with_euler_angles(roll: N, pitch: N, yaw: N) -> UnitQuat<N> {
|
||
let _0_5: N = Cast::from(0.5);
|
||
let (sr, cr) = (roll * _0_5).sin_cos();
|
||
let (sp, cp) = (pitch * _0_5).sin_cos();
|
||
let (sy, cy) = (yaw * _0_5).sin_cos();
|
||
|
||
unsafe {
|
||
UnitQuat::new_with_unit_quat(
|
||
Quat::new(
|
||
cr * cp * cy + sr * sp * sy,
|
||
sr * cp * cy - cr * sp * sy,
|
||
cr * sp * cy + sr * cp * sy,
|
||
cr * cp * sy - sr * sp * cy)
|
||
)
|
||
}
|
||
}
|
||
|
||
/// Builds a rotation matrix from this quaternion.
|
||
pub fn to_rot(&self) -> Rot3<N> {
|
||
let _2: N = Cast::from(2.0);
|
||
let ww = self.q.w * self.q.w;
|
||
let ii = self.q.i * self.q.i;
|
||
let jj = self.q.j * self.q.j;
|
||
let kk = self.q.k * self.q.k;
|
||
let ij = _2 * self.q.i * self.q.j;
|
||
let wk = _2 * self.q.w * self.q.k;
|
||
let wj = _2 * self.q.w * self.q.j;
|
||
let ik = _2 * self.q.i * self.q.k;
|
||
let jk = _2 * self.q.j * self.q.k;
|
||
let wi = _2 * self.q.w * self.q.i;
|
||
|
||
unsafe {
|
||
Rot3::new_with_mat(
|
||
Mat3::new(
|
||
ww + ii - jj - kk, ij - wk, wj + ik,
|
||
wk + ij, ww - ii + jj - kk, jk - wi,
|
||
ik - wj, wi + jk, ww - ii - jj + kk
|
||
)
|
||
)
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
impl<N> UnitQuat<N> {
|
||
/// Creates a new unit quaternion from a quaternion.
|
||
///
|
||
/// This is unsafe because the input quaternion will not be normalized.
|
||
#[inline]
|
||
pub unsafe fn new_with_unit_quat(q: Quat<N>) -> UnitQuat<N> {
|
||
UnitQuat {
|
||
q: q
|
||
}
|
||
}
|
||
|
||
/// The `Quat` representation of this unit quaternion.
|
||
#[inline]
|
||
pub fn quat<'a>(&'a self) -> &'a Quat<N> {
|
||
&self.q
|
||
}
|
||
}
|
||
|
||
impl<N: BaseNum> One for UnitQuat<N> {
|
||
#[inline]
|
||
fn one() -> UnitQuat<N> {
|
||
unsafe {
|
||
UnitQuat::new_with_unit_quat(Quat::new(::one(), ::zero(), ::zero(), ::zero()))
|
||
}
|
||
}
|
||
}
|
||
|
||
impl<N: Copy + Neg<Output = N>> Inv for UnitQuat<N> {
|
||
#[inline]
|
||
fn inv(&self) -> Option<UnitQuat<N>> {
|
||
let mut cpy = *self;
|
||
|
||
cpy.inv_mut();
|
||
Some(cpy)
|
||
}
|
||
|
||
#[inline]
|
||
fn inv_mut(&mut self) -> bool {
|
||
self.q.conjugate_mut();
|
||
|
||
true
|
||
}
|
||
}
|
||
|
||
impl<N: Rand + BaseFloat> Rand for UnitQuat<N> {
|
||
#[inline]
|
||
fn rand<R: Rng>(rng: &mut R) -> UnitQuat<N> {
|
||
UnitQuat::new(rng.gen())
|
||
}
|
||
}
|
||
|
||
impl<N: ApproxEq<N>> ApproxEq<N> for UnitQuat<N> {
|
||
#[inline]
|
||
fn approx_epsilon(_: Option<UnitQuat<N>>) -> N {
|
||
ApproxEq::approx_epsilon(None::<N>)
|
||
}
|
||
|
||
#[inline]
|
||
fn approx_ulps(_: Option<UnitQuat<N>>) -> u32 {
|
||
ApproxEq::approx_ulps(None::<N>)
|
||
}
|
||
|
||
#[inline]
|
||
fn approx_eq_eps(&self, other: &UnitQuat<N>, eps: &N) -> bool {
|
||
ApproxEq::approx_eq_eps(&self.q, &other.q, eps)
|
||
}
|
||
|
||
#[inline]
|
||
fn approx_eq_ulps(&self, other: &UnitQuat<N>, ulps: u32) -> bool {
|
||
ApproxEq::approx_eq_ulps(&self.q, &other.q, ulps)
|
||
}
|
||
}
|
||
|
||
impl<N: BaseFloat + ApproxEq<N>> Div<UnitQuat<N>> for UnitQuat<N> {
|
||
type Output = UnitQuat<N>;
|
||
|
||
#[inline]
|
||
fn div(self, other: UnitQuat<N>) -> UnitQuat<N> {
|
||
UnitQuat { q: self.q / other.q }
|
||
}
|
||
}
|
||
|
||
impl<N: BaseNum> Mul<UnitQuat<N>> for UnitQuat<N> {
|
||
type Output = UnitQuat<N>;
|
||
|
||
#[inline]
|
||
fn mul(self, right: UnitQuat<N>) -> UnitQuat<N> {
|
||
UnitQuat { q: self.q * right.q }
|
||
}
|
||
}
|
||
|
||
impl<N: BaseNum> Mul<Vec3<N>> for UnitQuat<N> {
|
||
type Output = Vec3<N>;
|
||
|
||
#[inline]
|
||
fn mul(self, right: Vec3<N>) -> Vec3<N> {
|
||
let _2: N = ::one::<N>() + ::one();
|
||
let mut t = ::cross(self.q.vector(), &right);
|
||
t.x = t.x * _2;
|
||
t.y = t.y * _2;
|
||
t.z = t.z * _2;
|
||
|
||
Vec3::new(t.x * self.q.w, t.y * self.q.w, t.z * self.q.w) + ::cross(self.q.vector(), &t) + right
|
||
}
|
||
}
|
||
|
||
impl<N: BaseNum> Mul<Pnt3<N>> for UnitQuat<N> {
|
||
type Output = Pnt3<N>;
|
||
|
||
#[inline]
|
||
fn mul(self, right: Pnt3<N>) -> Pnt3<N> {
|
||
::orig::<Pnt3<N>>() + self * *right.as_vec()
|
||
}
|
||
}
|
||
|
||
impl<N: BaseNum + Neg<Output = N>> Mul<UnitQuat<N>> for Vec3<N> {
|
||
type Output = Vec3<N>;
|
||
|
||
#[inline]
|
||
fn mul(self, right: UnitQuat<N>) -> Vec3<N> {
|
||
let mut inv_quat = right;
|
||
|
||
inv_quat.inv_mut();
|
||
|
||
inv_quat * self
|
||
}
|
||
}
|
||
|
||
impl<N: BaseNum + Neg<Output = N>> Mul<UnitQuat<N>> for Pnt3<N> {
|
||
type Output = Pnt3<N>;
|
||
|
||
#[inline]
|
||
fn mul(self, right: UnitQuat<N>) -> Pnt3<N> {
|
||
::orig::<Pnt3<N>>() + *self.as_vec() * right
|
||
}
|
||
}
|
||
|
||
impl<N: BaseFloat> Rotation<Vec3<N>> for UnitQuat<N> {
|
||
#[inline]
|
||
fn rotation(&self) -> Vec3<N> {
|
||
let _2 = ::one::<N>() + ::one();
|
||
let mut v = *self.q.vector();
|
||
let ang = _2 * v.normalize_mut().atan2(self.q.w);
|
||
|
||
if ::is_zero(&ang) {
|
||
::zero()
|
||
}
|
||
else {
|
||
Vec3::new(v.x * ang, v.y * ang, v.z * ang)
|
||
}
|
||
}
|
||
|
||
#[inline]
|
||
fn inv_rotation(&self) -> Vec3<N> {
|
||
-self.rotation()
|
||
}
|
||
|
||
#[inline]
|
||
fn append_rotation_mut(&mut self, amount: &Vec3<N>) {
|
||
*self = Rotation::append_rotation(self, amount)
|
||
}
|
||
|
||
#[inline]
|
||
fn append_rotation(&self, amount: &Vec3<N>) -> UnitQuat<N> {
|
||
*self * UnitQuat::new(*amount)
|
||
}
|
||
|
||
#[inline]
|
||
fn prepend_rotation_mut(&mut self, amount: &Vec3<N>) {
|
||
*self = Rotation::prepend_rotation(self, amount)
|
||
}
|
||
|
||
#[inline]
|
||
fn prepend_rotation(&self, amount: &Vec3<N>) -> UnitQuat<N> {
|
||
UnitQuat::new(*amount) * *self
|
||
}
|
||
|
||
#[inline]
|
||
fn set_rotation(&mut self, v: Vec3<N>) {
|
||
*self = UnitQuat::new(v)
|
||
}
|
||
}
|
||
|
||
impl<N: BaseFloat> RotationMatrix<N, Vec3<N>, Vec3<N>> for UnitQuat<N> {
|
||
type Output = Rot3<N>;
|
||
|
||
#[inline]
|
||
fn to_rot_mat(&self) -> Rot3<N> {
|
||
self.to_rot()
|
||
}
|
||
}
|
||
|
||
impl<N: BaseNum + Neg<Output = N>> Rotate<Vec3<N>> for UnitQuat<N> {
|
||
#[inline]
|
||
fn rotate(&self, v: &Vec3<N>) -> Vec3<N> {
|
||
*self * *v
|
||
}
|
||
|
||
#[inline]
|
||
fn inv_rotate(&self, v: &Vec3<N>) -> Vec3<N> {
|
||
*v * *self
|
||
}
|
||
}
|
||
|
||
impl<N: BaseNum + Neg<Output = N>> Rotate<Pnt3<N>> for UnitQuat<N> {
|
||
#[inline]
|
||
fn rotate(&self, p: &Pnt3<N>) -> Pnt3<N> {
|
||
*self * *p
|
||
}
|
||
|
||
#[inline]
|
||
fn inv_rotate(&self, p: &Pnt3<N>) -> Pnt3<N> {
|
||
*p * *self
|
||
}
|
||
}
|
||
|
||
impl<N: BaseFloat + ApproxEq<N>> RotationTo for UnitQuat<N> {
|
||
type AngleType = N;
|
||
type DeltaRotationType = UnitQuat<N>;
|
||
|
||
#[inline]
|
||
fn angle_to(&self, other: &Self) -> N {
|
||
let delta = self.rotation_to(other);
|
||
let _2 = ::one::<N>() + ::one();
|
||
|
||
_2 * delta.q.vector().norm().atan2(delta.q.w)
|
||
}
|
||
|
||
#[inline]
|
||
fn rotation_to(&self, other: &Self) -> UnitQuat<N> {
|
||
*other / *self
|
||
}
|
||
}
|
||
|
||
impl<N: BaseNum + Neg<Output = N>> Transform<Vec3<N>> for UnitQuat<N> {
|
||
#[inline]
|
||
fn transform(&self, v: &Vec3<N>) -> Vec3<N> {
|
||
*self * *v
|
||
}
|
||
|
||
#[inline]
|
||
fn inv_transform(&self, v: &Vec3<N>) -> Vec3<N> {
|
||
*v * *self
|
||
}
|
||
}
|
||
|
||
impl<N: BaseNum + Neg<Output = N>> Transform<Pnt3<N>> for UnitQuat<N> {
|
||
#[inline]
|
||
fn transform(&self, p: &Pnt3<N>) -> Pnt3<N> {
|
||
*self * *p
|
||
}
|
||
|
||
#[inline]
|
||
fn inv_transform(&self, p: &Pnt3<N>) -> Pnt3<N> {
|
||
*p * *self
|
||
}
|
||
}
|
||
|
||
#[cfg(feature="arbitrary")]
|
||
impl<N: Arbitrary + BaseFloat> Arbitrary for UnitQuat<N> {
|
||
fn arbitrary<G: Gen>(g: &mut G) -> UnitQuat<N> {
|
||
UnitQuat::new(Arbitrary::arbitrary(g))
|
||
}
|
||
}
|
||
|
||
|
||
pord_impl!(Quat, w, i, j, k);
|
||
vec_axis_impl!(Quat, w, i, j, k);
|
||
vec_cast_impl!(Quat, w, i, j, k);
|
||
conversion_impl!(Quat, 4);
|
||
index_impl!(Quat);
|
||
indexable_impl!(Quat, 4);
|
||
at_fast_impl!(Quat, 4);
|
||
repeat_impl!(Quat, val, w, i, j, k);
|
||
dim_impl!(Quat, 3);
|
||
container_impl!(Quat);
|
||
add_impl!(Quat, w, i, j, k);
|
||
sub_impl!(Quat, w, i, j, k);
|
||
scalar_add_impl!(Quat, w, i, j, k);
|
||
scalar_sub_impl!(Quat, w, i, j, k);
|
||
scalar_mul_impl!(Quat, w, i, j, k);
|
||
scalar_div_impl!(Quat, w, i, j, k);
|
||
neg_impl!(Quat, w, i, j, k);
|
||
zero_one_impl!(Quat, w, i, j, k);
|
||
approx_eq_impl!(Quat, w, i, j, k);
|
||
from_iterator_impl!(Quat, iterator, iterator, iterator, iterator);
|
||
bounded_impl!(Quat, w, i, j, k);
|
||
axpy_impl!(Quat, w, i, j, k);
|
||
iterable_impl!(Quat, 4);
|
||
iterable_mut_impl!(Quat, 4);
|
||
arbitrary_impl!(Quat, w, i, j, k);
|
||
|
||
dim_impl!(UnitQuat, 3);
|