use rand::{Rand, Rng};
use num_traits::cast;
use structure::*;
use angle::Rad;
use approx::ApproxEq;
use quaternion::Quaternion;
use num::BaseFloat;
#[repr(C)]
#[derive(Copy, Clone, Debug)]
#[derive(PartialEq, Eq)]
#[cfg_attr(feature = "eders", derive(Serialize, Deserialize))]
pub struct Euler<A: Angle> {
pub x: A,
pub y: A,
pub z: A,
}
impl<A: Angle> Euler<A> {
pub fn new(x: A, y: A, z: A) -> Euler<A> {
Euler { x: x, y: y, z: z }
}
}
impl<S: BaseFloat> From<Quaternion<S>> for Euler<Rad<S>> {
fn from(src: Quaternion<S>) -> Euler<Rad<S>> {
let sig: S = cast(0.499).unwrap();
let two: S = cast(2).unwrap();
let one: S = cast(1).unwrap();
let (qw, qx, qy, qz) = (src.s, src.v.x, src.v.y, src.v.z);
let (sqw, sqx, sqy, sqz) = (qw * qw, qx * qx, qy * qy, qz * qz);
let unit = sqx + sqz + sqy + sqw;
let test = qx * qz + qy * qw;
if test > sig * unit {
Euler {
x: Rad::zero(),
y: Rad::turn_div_4(),
z: Rad::atan2(qx, qw) * two,
}
} else if test < -sig * unit {
Euler {
x: Rad::zero(),
y: -Rad::turn_div_4(),
z: -Rad::atan2(qx, qw) * two,
}
} else {
Euler {
x: Rad::atan2(two * (-qy * qz + qx * qw), one - two * (sqx + sqy)),
y: Rad::asin(two * (qx * qz + qy * qw)),
z: Rad::atan2(two * (-qx * qy + qz * qw), one - two * (sqy + sqz)),
}
}
}
}
impl<A: Angle> ApproxEq for Euler<A> {
type Epsilon = A::Epsilon;
#[inline]
fn default_epsilon() -> A::Epsilon {
A::default_epsilon()
}
#[inline]
fn default_max_relative() -> A::Epsilon {
A::default_max_relative()
}
#[inline]
fn default_max_ulps() -> u32 {
A::default_max_ulps()
}
#[inline]
fn relative_eq(&self, other: &Self, epsilon: A::Epsilon, max_relative: A::Epsilon) -> bool {
A::relative_eq(&self.x, &other.x, epsilon, max_relative) &&
A::relative_eq(&self.y, &other.y, epsilon, max_relative) &&
A::relative_eq(&self.z, &other.z, epsilon, max_relative)
}
#[inline]
fn ulps_eq(&self, other: &Self, epsilon: A::Epsilon, max_ulps: u32) -> bool {
A::ulps_eq(&self.x, &other.x, epsilon, max_ulps) &&
A::ulps_eq(&self.y, &other.y, epsilon, max_ulps) &&
A::ulps_eq(&self.z, &other.z, epsilon, max_ulps)
}
}
impl<A: Angle + Rand> Rand for Euler<A> {
#[inline]
fn rand<R: Rng>(rng: &mut R) -> Euler<A> {
Euler { x: rng.gen(), y: rng.gen(), z: rng.gen() }
}
}