use std::f64;
use std::fmt;
use std::mem;
use std::ops::*;
use rand::{Rand, Rng};
use rust_num::{Float, one, zero};
use rust_num::traits::cast;
use angle::{Angle, Rad, acos, sin, sin_cos, rad};
use approx::ApproxEq;
use array::Array1;
use matrix::{Matrix3, Matrix4};
use num::BaseFloat;
use point::Point3;
use rotation::{Rotation, Rotation3, Basis3};
use vector::{Vector3, Vector, EuclideanVector};
#[derive(Copy, Clone, PartialEq, RustcEncodable, RustcDecodable)]
pub struct Quaternion<S> { pub s: S, pub v: Vector3<S> }
impl<S: Copy + BaseFloat> Array1<S> for Quaternion<S> {
#[inline]
fn map<F>(&mut self, mut op: F) -> Quaternion<S> where F: FnMut(S) -> S {
self.s = op(self.s);
self.v.x = op(self.v.x);
self.v.y = op(self.v.y);
self.v.z = op(self.v.z);
*self
}
}
impl<S: BaseFloat> Index<usize> for Quaternion<S> {
type Output = S;
#[inline]
fn index<'a>(&'a self, i: usize) -> &'a S {
let slice: &[S; 4] = unsafe { mem::transmute(self) };
&slice[i]
}
}
impl<S: BaseFloat> IndexMut<usize> for Quaternion<S> {
#[inline]
fn index_mut<'a>(&'a mut self, i: usize) -> &'a mut S {
let slice: &'a mut [S; 4] = unsafe { mem::transmute(self) };
&mut slice[i]
}
}
impl<S: BaseFloat> Quaternion<S> {
#[inline]
pub fn new(w: S, xi: S, yj: S, zk: S) -> Quaternion<S> {
Quaternion::from_sv(w, Vector3::new(xi, yj, zk))
}
#[inline]
pub fn from_sv(s: S, v: Vector3<S>) -> Quaternion<S> {
Quaternion { s: s, v: v }
}
#[inline]
pub fn zero() -> Quaternion<S> {
Quaternion::new(zero(), zero(), zero(), zero())
}
#[inline]
pub fn identity() -> Quaternion<S> {
Quaternion::from_sv(one::<S>(), zero())
}
#[inline]
pub fn mul_s(&self, value: S) -> Quaternion<S> {
Quaternion::from_sv(self.s * value, self.v.mul_s(value))
}
#[inline]
pub fn div_s(&self, value: S) -> Quaternion<S> {
Quaternion::from_sv(self.s / value, self.v.div_s(value))
}
#[inline]
pub fn mul_v(&self, vec: &Vector3<S>) -> Vector3<S> {
let tmp = self.v.cross(vec).add_v(&vec.mul_s(self.s.clone()));
self.v.cross(&tmp).mul_s(cast(2i8).unwrap()).add_v(vec)
}
#[inline]
pub fn add_q(&self, other: &Quaternion<S>) -> Quaternion<S> {
Quaternion::from_sv(self.s + other.s, self.v + other.v)
}
#[inline]
pub fn sub_q(&self, other: &Quaternion<S>) -> Quaternion<S> {
Quaternion::from_sv(self.s - other.s, self.v - other.v)
}
pub fn mul_q(&self, other: &Quaternion<S>) -> Quaternion<S> {
Quaternion::new(self.s * other.s - self.v.x * other.v.x - self.v.y * other.v.y - self.v.z * other.v.z,
self.s * other.v.x + self.v.x * other.s + self.v.y * other.v.z - self.v.z * other.v.y,
self.s * other.v.y + self.v.y * other.s + self.v.z * other.v.x - self.v.x * other.v.z,
self.s * other.v.z + self.v.z * other.s + self.v.x * other.v.y - self.v.y * other.v.x)
}
#[inline]
pub fn mul_self_s(&mut self, s: S) {
self.s = self.s * s;
self.v.mul_self_s(s);
}
#[inline]
pub fn div_self_s(&mut self, s: S) {
self.s = self.s / s;
self.v.div_self_s(s);
}
#[inline]
pub fn add_self_q(&mut self, q: &Quaternion<S>) {
self.s = self.s + q.s;
self.v.add_self_v(&q.v);
}
#[inline]
pub fn sub_self_q(&mut self, q: &Quaternion<S>) {
self.s = self.s - q.s;
self.v.sub_self_v(&q.v);
}
#[inline]
pub fn mul_self_q(&mut self, q: &Quaternion<S>) {
self.s = self.s * q.s;
self.v.mul_self_v(&q.v);
}
#[inline]
pub fn dot(&self, q: &Quaternion<S>) -> S {
self.s * q.s + self.v.dot(&q.v)
}
#[inline]
pub fn conjugate(&self) -> Quaternion<S> {
Quaternion::from_sv(self.s.clone(), -self.v.clone())
}
#[inline]
pub fn magnitude2(&self) -> S {
self.s * self.s + self.v.length2()
}
#[inline]
pub fn magnitude(&self) -> S {
self.magnitude2().sqrt()
}
#[inline]
pub fn normalize(&self) -> Quaternion<S> {
self.mul_s(one::<S>() / self.magnitude())
}
pub fn nlerp(&self, other: &Quaternion<S>, amount: S) -> Quaternion<S> {
self.mul_s(one::<S>() - amount).add_q(&other.mul_s(amount)).normalize()
}
}
impl<S: BaseFloat> ApproxEq<S> for Quaternion<S> {
#[inline]
fn approx_eq_eps(&self, other: &Quaternion<S>, epsilon: &S) -> bool {
self.s.approx_eq_eps(&other.s, epsilon) &&
self.v.approx_eq_eps(&other.v, epsilon)
}
}
impl<S: BaseFloat> Quaternion<S> {
pub fn slerp(&self, other: &Quaternion<S>, amount: S) -> Quaternion<S> {
let dot = self.dot(other);
let dot_threshold = cast(0.9995f64).unwrap();
if dot > dot_threshold {
self.nlerp(other, amount)
} else {
let robust_dot = if dot > one::<S>() {
one::<S>()
} else if dot < -one::<S>() {
-one::<S>()
} else {
dot
};
let theta: Rad<S> = acos(robust_dot.clone());
let scale1 = sin(theta.mul_s(one::<S>() - amount));
let scale2 = sin(theta.mul_s(amount));
self.mul_s(scale1)
.add_q(&other.mul_s(scale2))
.mul_s(sin(theta).recip())
}
}
pub fn to_euler(&self) -> (Rad<S>, Rad<S>, Rad<S>) {
let sig: S = cast(0.499f64).unwrap();
let two: S = cast(2f64).unwrap();
let one: S = cast(1f64).unwrap();
let (qw, qx, qy, qz) = (self.s, self.v.x, self.v.y, self.v.z);
let (sqw, sqx, sqy, sqz) = (qw*qw, qx*qx, qy*qy, qz*qz);
let unit = sqx + sqy + sqz + sqw;
let test = qx*qy + qz*qw;
if test > sig * unit {
(
rad(zero::<S>()),
rad(cast(f64::consts::FRAC_PI_2).unwrap()),
rad(two * qx.atan2(qw)),
)
} else if test < -sig * unit {
let y: S = cast(f64::consts::FRAC_PI_2).unwrap();
(
rad(zero::<S>()),
rad(-y),
rad(two * qx.atan2(qw)),
)
} else {
(
rad((two * (qy*qw - qx*qz)).atan2(one - two*(sqy + sqz))),
rad((two * (qx*qy + qz*qw)).asin()),
rad((two * (qx*qw - qy*qz)).atan2(one - two*(sqx + sqz))),
)
}
}
}
impl<S: BaseFloat> From<Quaternion<S>> for Matrix3<S> {
fn from(quat: Quaternion<S>) -> Matrix3<S> {
let x2 = quat.v.x + quat.v.x;
let y2 = quat.v.y + quat.v.y;
let z2 = quat.v.z + quat.v.z;
let xx2 = x2 * quat.v.x;
let xy2 = x2 * quat.v.y;
let xz2 = x2 * quat.v.z;
let yy2 = y2 * quat.v.y;
let yz2 = y2 * quat.v.z;
let zz2 = z2 * quat.v.z;
let sy2 = y2 * quat.s;
let sz2 = z2 * quat.s;
let sx2 = x2 * quat.s;
Matrix3::new(one::<S>() - yy2 - zz2, xy2 + sz2, xz2 - sy2,
xy2 - sz2, one::<S>() - xx2 - zz2, yz2 + sx2,
xz2 + sy2, yz2 - sx2, one::<S>() - xx2 - yy2)
}
}
impl<S: BaseFloat> From<Quaternion<S>> for Matrix4<S> {
fn from(quat: Quaternion<S>) -> Matrix4<S> {
let x2 = quat.v.x + quat.v.x;
let y2 = quat.v.y + quat.v.y;
let z2 = quat.v.z + quat.v.z;
let xx2 = x2 * quat.v.x;
let xy2 = x2 * quat.v.y;
let xz2 = x2 * quat.v.z;
let yy2 = y2 * quat.v.y;
let yz2 = y2 * quat.v.z;
let zz2 = z2 * quat.v.z;
let sy2 = y2 * quat.s;
let sz2 = z2 * quat.s;
let sx2 = x2 * quat.s;
Matrix4::new(one::<S>() - yy2 - zz2, xy2 + sz2, xz2 - sy2, zero::<S>(),
xy2 - sz2, one::<S>() - xx2 - zz2, yz2 + sx2, zero::<S>(),
xz2 + sy2, yz2 - sx2, one::<S>() - xx2 - yy2, zero::<S>(),
zero::<S>(), zero::<S>(), zero::<S>(), one::<S>())
}
}
impl<S: BaseFloat> Neg for Quaternion<S> {
type Output = Quaternion<S>;
#[inline]
fn neg(self) -> Quaternion<S> {
Quaternion::from_sv(-self.s, -self.v)
}
}
impl<S: BaseFloat> fmt::Debug for Quaternion<S> {
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
write!(f, "{:?} + {:?}i + {:?}j + {:?}k",
self.s,
self.v.x,
self.v.y,
self.v.z)
}
}
impl<S: BaseFloat> From<Quaternion<S>> for Basis3<S> {
#[inline]
fn from(quat: Quaternion<S>) -> Basis3<S> { Basis3::from_quaternion(&quat) }
}
impl<S: BaseFloat + 'static> Rotation<S, Vector3<S>, Point3<S>> for Quaternion<S> {
#[inline]
fn identity() -> Quaternion<S> { Quaternion::identity() }
#[inline]
fn look_at(dir: &Vector3<S>, up: &Vector3<S>) -> Quaternion<S> {
Matrix3::look_at(dir, up).into()
}
#[inline]
fn between_vectors(a: &Vector3<S>, b: &Vector3<S>) -> Quaternion<S> {
Quaternion::from_sv(one::<S>() + a.dot(b), a.cross(b)).normalize()
}
#[inline]
fn rotate_vector(&self, vec: &Vector3<S>) -> Vector3<S> { self.mul_v(vec) }
#[inline]
fn concat(&self, other: &Quaternion<S>) -> Quaternion<S> { self.mul_q(other) }
#[inline]
fn concat_self(&mut self, other: &Quaternion<S>) { self.mul_self_q(other); }
#[inline]
fn invert(&self) -> Quaternion<S> { self.conjugate().div_s(self.magnitude2()) }
#[inline]
fn invert_self(&mut self) { *self = self.invert() }
}
impl<S: BaseFloat> Rotation3<S> for Quaternion<S> where S: 'static {
#[inline]
fn from_axis_angle(axis: &Vector3<S>, angle: Rad<S>) -> Quaternion<S> {
let (s, c) = sin_cos(angle.mul_s(cast(0.5f64).unwrap()));
Quaternion::from_sv(c, axis.mul_s(s))
}
fn from_euler(x: Rad<S>, y: Rad<S>, z: Rad<S>) -> Quaternion<S> {
let (s1, c1) = sin_cos(x.mul_s(cast(0.5f64).unwrap()));
let (s2, c2) = sin_cos(y.mul_s(cast(0.5f64).unwrap()));
let (s3, c3) = sin_cos(z.mul_s(cast(0.5f64).unwrap()));
Quaternion::new(c1 * c2 * c3 - s1 * s2 * s3,
s1 * s2 * c3 + c1 * c2 * s3,
s1 * c2 * c3 + c1 * s2 * s3,
c1 * s2 * c3 - s1 * c2 * s3)
}
}
impl<S: BaseFloat + Rand> Rand for Quaternion<S> {
#[inline]
fn rand<R: Rng>(rng: &mut R) -> Quaternion<S> {
Quaternion::from_sv(rng.gen(), rng.gen())
}
}