gml 1.1.0

// Copyright 2015 Creekware Inc. See the COPYRIGHT
// file at the top-level directory of this distribution.
//
// Licensed under the Apache License, Version 2.0 <LICENSE-APACHE or
// http://www.apache.org/licenses/LICENSE-2.0> or the MIT license
// <LICENSE-MIT or http://opensource.org/licenses/MIT>, at your
// option. This file may not be copied, modified, or distributed
// except according to those terms.

//! Defines a templated quaternion type and several quaternion operations.
//!
//! Quaternion are used to represent rotations.  They are compact, can be
//! easily interpolated and don't suffer from gimbal lock.
//!
//! Quaternions are based on complex numbers and are not intuative.
//! The individual Quaternion components (x, y, z, w) are almost never
//! accessed or modified directly; more common is to take existing
//! rotations and use them to construct new rotations (for example to
//! smoothly interpolate between two rotations).

extern crate num;

use num::traits::*;
use std::ops::*;
use std::mem;
use vector::*;
use matrix::*;

#[derive(Debug, Clone, Copy, PartialEq)]
#[repr(C)]
pub struct Quaternion<T> { pub x:T, pub y:T, pub z:T, pub w:T }

/// A quaternion of `f32` types.
pub type Quaternionf = Quaternion<f32>;

/// A quaternion of `f64` types.
pub type Quaterniond = Quaternion<f32>;

impl<T:Copy> Quaternion<T> {

    // Constructors

    /// Construct a new quaternion from one scalar component and
    /// three imaginary components
    ///
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternion::new(2.0, 3.0, 4.0, 5.0);
    ///
    /// assert!(q.x == 2.0);
    /// assert!(q.y == 3.0);
    /// assert!(q.z == 4.0);
    /// assert!(q.w == 5.0);
    /// ```
    pub fn new(x:T, y:T, z:T, w:T ) -> Quaternion<T> {
        Quaternion{ w:w, x:x, y:y, z:z }
    }
    
    /// Construct a new quaternion with all values set to the same scalar component.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternion::new_s(2.0);
    ///
    /// assert!(q.x == 2.0);
    /// assert!(q.y == 2.0);
    /// assert!(q.z == 2.0);
    /// assert!(q.w == 2.0);
    /// ```
    pub fn new_s(s:T) -> Quaternion<T> {
        Quaternion{ x:s, y:s, z:s, w:s }
    }
    
    /// Construct a new quaternion from one scalar component and
    /// three imaginary components taken from a three-element vector.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let v = Vector3::new(2.0, 3.0, 4.0);
    /// let q = Quaternion::new_v3(v, 5.0);
    ///
    /// assert!(q.x == 2.0);
    /// assert!(q.y == 3.0);
    /// assert!(q.z == 4.0);
    /// assert!(q.w == 5.0);
    /// ```
    pub fn new_v3(v:Vector3<T>, w:T ) -> Quaternion<T> {
        Quaternion{ x:v.x, y:v.y, z:v.z, w:w }
    }

    /// Return the number of elements in a quaternion.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternion::new_s(5.0);
    ///
    /// assert!(q.len() == 4);
    /// ```
    pub fn len(self) -> usize { 4 }

}


// Default

impl<T:Copy + Zero + One> Default for Quaternion<T> {

    /// Return the default (identity) quaternerion 
    ///
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternionf::default();
    ///
    /// assert!(q.x == 0.0);
    /// assert!(q.y == 0.0);
    /// assert!(q.z == 0.0);
    /// assert!(q.w == 1.0);
    ///
    /// ```
    fn default() -> Quaternion<T> {
        Quaternion{ x:T::zero(), 
                    y:T::zero(), 
                    z:T::zero(),
                    w:T::one()   }
    }
}

impl<T:Copy + Zero + One> Quaternion<T> {

    /// Return the identity quaternerion
    ///
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternionf::identity();
    ///
    /// assert!(q.x == 0.0);
    /// assert!(q.y == 0.0);
    /// assert!(q.z == 0.0);
    /// assert!(q.w == 1.0);
    ///
    /// ```
    pub fn identity() -> Quaternion<T> {
        Quaternion{ x:T::zero(), 
                    y:T::zero(), 
                    z:T::zero(),
                    w:T::one()   }
    }
}

// Implementation of Zero

impl<T:Copy + Zero + PartialEq> Zero for Quaternion<T> {

    /// Return a quaternion with all elements initialize to zero.
    ///
    fn zero() -> Quaternion<T> {
        Quaternion{ x:T::zero(), 
                    y:T::zero(), 
                    z:T::zero(),
                    w:T::zero() }
    }

    /// Return true if all elements of a quaternion are equal to zero;
    /// false otherwise.
    ///
    fn is_zero(&self) -> bool {
        self.x == T::zero() && 
            self.y == T::zero() && 
            self.z == T::zero() &&
            self.w == T::zero() 
    }
}

// Implementation of One

impl<T:Copy + Num> One for Quaternion<T> {

    /// Return a quaternion with all elements initialize to one.
    ///
    fn one() -> Quaternion<T> {
        Quaternion{ x: T::one(), y: T::one(), z: T::one(), w: T::one() }
    }
}

// Operator overrides

impl<T:Copy> Index<usize> for Quaternion<T> {
    type Output = T;

    /// Turns the quaternion into a fixed size array, and allows that array to be
    /// indexed.  The returned values are immutable.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternion::new(2,4,6,8);
    ///
    /// assert!(q[0] == 2 && q.x == 2);
    /// assert!(q[1] == 4 && q.y == 4);
    /// assert!(q[2] == 6 && q.z == 6);
    /// assert!(q[3] == 8 && q.w == 8);
    /// ```
    fn index<'a>(&'a self, idx:usize) -> &'a T {
        let s: &[T;4] = unsafe { mem::transmute(self) };
        &s[idx]
    }
}

impl<T:Copy> IndexMut<usize> for Quaternion<T> {

    /// Turns the quaternion into a fixed size array, and allows that array to be
    /// indexed.  The returned values are mutable.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let mut q = Quaternionf::default();
    ///
    /// q[0] = 123.0;
    /// q[1] = 456.0;
    /// q[2] = 789.0;
    /// q[3] = 482.0;
    ///
    /// assert!(q[0] == 123.0 && q.x == 123.0);
    /// assert!(q[1] == 456.0 && q.y == 456.0);
    /// assert!(q[2] == 789.0 && q.z == 789.0);
    /// assert!(q[3] == 482.0 && q.w == 482.0);
    /// ```
    fn index_mut<'a>(&'a mut self, idx:usize) -> &'a mut T {
        let s: &'a mut [T;4] = unsafe { mem::transmute(self) };
        &mut s[idx]
    }
}

impl<T:Add<Output = T> + Zero + Copy> Add<Quaternion<T>> for Quaternion<T> {
    type Output = Quaternion<T>;

    /// Returns a new quaternion in which each element is the sum of
    /// the corresponding element from `self` and `rhs`.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let a = Quaternion::new(2, 4, 6, 8);
    /// let b = Quaternion::new(1, 3, 5, 7);
    ///
    /// let r = a + b; assert!(r == Quaternion::new(3, 7, 11, 15));
    /// ```
    fn add(self, rhs:Quaternion<T>) -> Quaternion<T> {
        Quaternion{x:self.x + rhs.x,
                   y:self.y + rhs.y,
                   z:self.z + rhs.z,
                   w:self.w + rhs.w }
    }
}

impl<T: Add<Output=T> + Zero + Copy> Add<T> for Quaternion<T> {
    type Output = Quaternion<T>;
    
    /// Returns a new quaternion in which each element is the sum of
    /// the corresponding element from `self` and the argument `s`.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let a = Quaternion::new(2, 4, 6, 8);
    ///
    /// let r = a + 10; assert!(r == Quaternion::new(12, 14, 16, 18));
    /// ```
    fn add(self, s:T) -> Quaternion<T> {
        Quaternion{x:self.x + s,
                   y:self.y + s,
                   z:self.z + s,
                   w:self.w + s }
    }
}

impl<T:Copy + Num> Mul<Quaternion<T>> for Quaternion<T> {
    type Output = Quaternion<T>;

    /// Returns the product of two quaternions.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let a = Quaternion::new(2, 3, 4, 5);
    /// let b = Quaternion::new(6, 7, 8, 9);
    /// let r = a * b;
    ///
    /// assert!(r == Quaternion::new(44, 70, 72, -20));
    /// ```
    fn mul(self, rhs:Quaternion<T>) -> Quaternion<T> {
        Quaternion{x: self.w * rhs.x + self.x * rhs.w + self.y * rhs.z - self.z * rhs.y,
                   y: self.w * rhs.y + self.y * rhs.w + self.z * rhs.x - self.x * rhs.z,
                   z: self.w * rhs.z + self.z * rhs.w + self.x * rhs.y - self.y * rhs.x,
                   w: self.w * rhs.w - self.x * rhs.x - self.y * rhs.y - self.z * rhs.z
        }
    }
}

fn mul_v3<T:Copy + Num>(q:Quaternion<T>, v:Vector3<T>) -> Vector3<T> {
    let qv = Vector3::new(q.x, q.y, q.z);
    let uv = qv.cross(v);
    let uuv = qv.cross(uv);
    let two = T::one() + T::one();

    v + ((uv * q.w) + uuv) * two
}

impl<T:Copy + Num> Mul<Vector3<T>> for Quaternion<T> {
    type Output = Vector3<T>;
    
    /// Returns the product a quaternion with a three-element vector.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternion::new(2, 3, 4, 5);
    /// let v = Vector3::new(6, 7, 8);
    /// let r = q * v;
    ///
    /// assert!(r == Vector3::new(-122, 71, 24));
    /// ```
    fn mul(self, v:Vector3<T>) -> Vector3<T> {
        mul_v3(self, v)
    }
}

impl<T:Copy + Num> Mul<Vector4<T>> for Quaternion<T> {
    type Output = Vector4<T>;
    
    /// Returns the product a quaternion with a four-element vector.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternion::new(2, 3, 4, 5);
    /// let v = Vector4::new(6, 7, 8, 9);
    /// let r = q * v;
    ///
    /// assert!(r == Vector4::new(-122, 71, 24, 9));
    /// ```
    fn mul(self, v:Vector4<T>) -> Vector4<T> {
        Vector4::new_v3( self * Vector3::new_v4(v), v.w )
    }
}

impl<T:Mul<Output = T> + Zero + Copy> Mul<T> for Quaternion<T> {
    type Output = Quaternion<T>;
    
    /// Returns the product a quaternion with a scalar.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternion::new(2, 3, 4, 5);
    /// let r = q * 10;
    ///
    /// assert!(r == Quaternion::new(20, 30, 40, 50));
    /// ```
    fn mul(self, s:T) -> Quaternion<T> {
        Quaternion{x:self.x * s,
                   y:self.y * s,
                   z:self.z * s,
                   w:self.w * s }
    }
}

impl<T:Copy + Float> Div<T> for Quaternion<T> {
    type Output = Quaternion<T>;
    
    /// Returns the product a quaternion with a scalar.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternion::new(2.0, 4.0, 6.0, 8.0);
    /// let r = q / 2.0;
    ///
    /// assert!(r == Quaternion::new(1.0, 2.0, 3.0, 4.0));
    /// ```
    fn div(self, s:T) -> Quaternion<T> {
        Quaternion{x:self.x / s,
                   y:self.y / s,
                   z:self.z / s,
                   w:self.w / s }
    }
}

impl<T:Copy + Neg<Output=T> > Neg for Quaternion<T> {
    type Output = Quaternion<T>;

    /// Negates a quat.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternion::new(2.0, 4.0, 6.0, 8.0);
    /// let r = -q;
    ///
    /// assert!(r == Quaternion::new(-2.0, -4.0, -6.0, -8.0));
    /// ```
    fn neg(self) -> Quaternion<T> {
        Quaternion{x: -self.x,
                   y: -self.y,
                   z: -self.z,
                   w: -self.w }
    }
}

impl<T:Copy + Mul<Output = T> + Add<Output = T> + Sub<Output = T> + PartialEq> Quaternion<T> {

    /// Returns dot product of q1 and q2, i.e., q1[0] * q2[0] + q1[1] * q2[1] + ...
    ///
    /// ```
    /// use gml::*;
    ///
    /// let a = Quaternion::new(2, 3, 4, 5);
    /// let b = Quaternion::new(6, 7, 8, 9);
    /// let r = a.dot(b);
    ///
    /// assert!(r == 110);
    /// ```
    pub fn dot(self, rhs:Quaternion<T>) -> T {
        self.x * rhs.x + 
            self.y * rhs.y + 
            self.z * rhs.z + 
            self.w * rhs.w
    }

    /// Returns the cross product of two quats.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let a = Quaternion::new(2, 3, 4, 5);
    /// let b = Quaternion::new(6, 7, 8, 9);
    /// let r = a.cross(b);
    ///
    /// assert!(r == Quaternion::new(44, 70, 72, -20));
    /// ```
    pub fn cross(self, rhs:Quaternion<T>) -> Quaternion<T> {
        Quaternion{ 
            x:self.w * rhs.x + self.x * rhs.w + self.y * rhs.z - self.z * rhs.y,
            y:self.w * rhs.y + self.y * rhs.w + self.z * rhs.x - self.x * rhs.z,
            z:self.w * rhs.z + self.z * rhs.w + self.x * rhs.y - self.y * rhs.x,
            w:self.w * rhs.w - self.x * rhs.x - self.y * rhs.y - self.z * rhs.z }
    }
}


impl<T:Copy + Float> Quaternion<T> {
    /// Create a quaternion from two normalized axis
    /// 
    /// u
    /// : A first normalized axis
    ///
    /// v
    /// : A second normalized axis
    ///
    /// @see http://lolengine.net/blog/2013/09/18/beautiful-maths-quaternion-from-vectors
    ///
    /// ```
    /// use gml::*;
    ///
    /// let u = Vector3::new(2.0, 3.0, 4.0);
    /// let v = Vector3::new(5.0, 6.0, 7.0);
    /// let q = Quaternion::new_axis(u, v);
    ///
    /// assert!(eq_approx(q.x, -0.0522  , 1e-5));
    /// assert!(eq_approx(q.y,  0.104399, 1e-5));
    /// assert!(eq_approx(q.z, -0.0522  , 1e-5));
    /// assert!(eq_approx(q.w,  0.991792, 1e-5));
    /// ```
    pub fn new_axis(u:Vector3<T>, v:Vector3<T>) -> Quaternion<T> {
        let w   = u.cross(v);
        Quaternion::new(w[0], w[1], w[2], T::one() + u.dot(v)).normalize()
    }
    
    /// Build a quaternion from an angle and a normalized axis.
    ///
    /// _angle_: Angle expressed in radians.
    ///
    /// _axis_: Axis of the quaternion, must be normalized.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let v = Vector3::new( 7.0, 8.0, 9.0 );
    /// let r = Quaternion::new_angle_axis(0.2, v);
    ///
    /// assert!(eq_approx(r.x, 0.698834, 1e-5));
    /// assert!(eq_approx(r.y, 0.798667, 1e-5));
    /// assert!(eq_approx(r.z, 0.898501, 1e-5));
    /// assert!(eq_approx(r.w, 0.995004, 1e-5));
    /// ```
    pub fn new_angle_axis(angle:T, axis:Vector3<T>) -> Quaternion<T> {
        let mut result = Quaternion::default();

        let a = angle;
        let s = (a * T::from(0.5).unwrap()).sin();

        result.w = (a * T::from(0.5).unwrap()).cos();
        result.x = axis.x * s;
        result.y = axis.y * s;
        result.z = axis.z * s;
        return result;
    }

    /// Build a quaternion from euler angles (pitch, yaw, roll), in radians, stored in a three-element vector.
    /// The resultant quaternion rotates z degrees around the z axis, x degrees
    /// around the x axis, and y degrees around the y axis (in that order).
    ///
    /// ```
    /// use gml::*;
    ///
    /// // A rotation 0.2 degrees around x axis, 0.3 degrees around y axis, and 0.4
    /// // degrees around the z axis.
    ///
    /// let v = Vector3::new(0.2, 0.3, 0.4);
    /// let q = Quaternion::new_euler(v);
    ///
    /// assert!(eq_approx(q.x, 0.067204, 1e-5));
    /// assert!(eq_approx(q.y, 0.165339, 1e-5));
    /// assert!(eq_approx(q.z, 0.180836, 1e-5));
    /// assert!(eq_approx(q.w, 0.967184, 1e-5));
    /// ```
    pub fn new_euler(euler_angles:Vector3<T>) -> Quaternion<T> {
        let euler_half = euler_angles / T::from(2).unwrap();
        let c = euler_half.cos();
        let s = euler_half.sin();
        Quaternion{x: s.x * c.y * c.z - c.x * s.y * s.z,
                   y: c.x * s.y * c.z + s.x * c.y * s.z,
                   z: c.x * c.y * s.z - s.x * s.y * c.z,
                   w: c.x * c.y * c.z + s.x * s.y * s.z }
    }

    /// Create a quaternion from a 3x3 matrix.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let m = Matrix3x3::new(1.0, 2.0, 3.0,
    ///                   4.0, 5.0, 6.0,
    ///                   7.0, 8.0, 9.0);
    /// let q = Quaternion::new_m3(m);
    ///
    /// assert!(q.x == -0.25);
    /// assert!(q.y ==  0.5 );
    /// assert!(q.z == -0.25);
    /// assert!(q.w ==  2.0 );
    /// ```
    pub fn new_m3(m:Matrix3x3<T>) -> Quaternion<T> {
        let four_x_squared_minus_1 = m[0][0] - m[1][1] - m[2][2];
        let four_y_squared_minus_1 = m[1][1] - m[0][0] - m[2][2];
        let four_z_squared_minus_1 = m[2][2] - m[0][0] - m[1][1];
        let four_w_squared_minus_1 = m[0][0] + m[1][1] + m[2][2];

        let mut biggest_index = 0;
        let mut four_biggest_squared_minus_1 = four_w_squared_minus_1;
        if four_x_squared_minus_1 > four_biggest_squared_minus_1 {
            four_biggest_squared_minus_1 = four_x_squared_minus_1;
            biggest_index = 1;
        }
        if four_y_squared_minus_1 > four_biggest_squared_minus_1 {
            four_biggest_squared_minus_1 = four_y_squared_minus_1;
            biggest_index = 2;
        }
        if four_z_squared_minus_1 > four_biggest_squared_minus_1 {
            four_biggest_squared_minus_1 = four_z_squared_minus_1;
            biggest_index = 3;
        }

        let biggest_val = (four_biggest_squared_minus_1 + T::one()).sqrt() * T::from(0.5).unwrap();
        let mult = T::from(0.25).unwrap() / biggest_val;

        let mut result = Quaternion::zero();
        match biggest_index {
            0 => { result.w = biggest_val;
                   result.x = (m[1][2] - m[2][1]) * mult;
                   result.y = (m[2][0] - m[0][2]) * mult;
                   result.z = (m[0][1] - m[1][0]) * mult;
            }
            1 => { result.w = (m[1][2] - m[2][1]) * mult;
                   result.x = biggest_val;
                   result.y = (m[0][1] + m[1][0]) * mult;
                   result.z = (m[2][0] + m[0][2]) * mult;
            }
            2 => { result.w = (m[2][0] - m[0][2]) * mult;
                   result.x = (m[0][1] + m[1][0]) * mult;
                   result.y = biggest_val;
                   result.z = (m[1][2] + m[2][1]) * mult;
            }
            3 => { result.w = (m[0][1] - m[1][0]) * mult;
                   result.x = (m[2][0] + m[0][2]) * mult;
                   result.y = (m[1][2] + m[2][1]) * mult;
                   result.z = biggest_val;
            }
            _ => {
                unreachable!();
            }
        }
        return result;
    }
    
    /// Create a quaternion from a 4x4 matrix.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let m = Matrix4x4::new(1.0, 2.0, 3.0,11.0,
    ///                   4.0, 5.0, 6.0,12.0,
    ///                   7.0, 8.0, 9.0,13.0,
    ///                  14.0,15.0,16.0,17.0);
    /// let q = Quaternion::new_m4(m);
    ///
    /// assert!(q.x == -0.25);
    /// assert!(q.y ==  0.5 );
    /// assert!(q.z == -0.25);
    /// assert!(q.w ==  2.0 );
    /// ```
    pub fn new_m4(m:Matrix4x4<T>) -> Quaternion<T> {
        Quaternion::new_m3( Matrix3x3::new_m4x4(m) )
    }

    /// Returns the length of the quaternion.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternion::new( 3.0,  4.0, 12.0, 0.0);
    ///
    /// assert!( q.length() == 13.0);
    /// ```
    pub fn length(self) -> T {
        self.dot(self).sqrt()
    }
    
    /// Returns the normalized quaternion.
    /// 
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternion::new( 3.0,  4.0, 12.0, 0.0);
    /// let r = q.normalize();
    ///
    /// assert!(eq_approx(r.x, 0.230769, 1e-5));
    /// assert!(eq_approx(r.y, 0.307692, 1e-5));
    /// assert!(eq_approx(r.z, 0.923077, 1e-5));
    /// assert!(eq_approx(r.w, 0.0     , 1e-5));
    /// ```
    pub fn normalize(self) -> Quaternion<T> {
        let len = self.length();
        if len <= T::zero() { // problem
            return Quaternion{x:T::one(), y:T::zero(), z:T::zero(), w:T::zero()};
        }
        let one_over_len = T::one() / len;
        return Quaternion{x:self.x * one_over_len,
                          y:self.y * one_over_len,
                          z:self.z * one_over_len,
                          w:self.w * one_over_len };
    }

    fn mix_t(x:T, y:T, a:T) -> T {
        x * (T::one() - a) + y * a
    }
    

    /// Spherical linear interpolation of two quaternions.
    /// The interpolation is oriented and the rotation is performed at constant speed.
    /// For short path spherical linear interpolation, use the slerp function.
    /// 
    /// _x_
    /// :    A quaternion
    ///
    /// _y_
    /// :    A quaternion
    ///
    /// _a_
    /// :    Interpolation factor. The interpolation is defined beyond the range [0, 1].
    ///
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternion::new( 3.0,  4.0, 5.0, 6.0);
    /// let y = Quaternion::new( 7.0,  8.0, 9.0,10.0);
    /// let r = q.mix(y, 0.2);
    ///
    /// assert!(eq_approx(r.x, 3.8, 1e-5));
    /// assert!(eq_approx(r.y, 4.8, 1e-5));
    /// assert!(eq_approx(r.z, 5.8, 1e-5));
    /// assert!(eq_approx(r.w, 6.8, 1e-5));
    /// ```
    pub fn mix(self, y:Quaternion<T>, a:T) -> Quaternion<T> {
        let cos_theta = self.dot(y);

        // Perform a linear interpolation when cosTheta is close to 1 to avoid side effect of sin(angle) becoming a zero denominator
        if cos_theta > T::one() - T::one()/*epsilon<T>()*/ {
            // Linear interpolation
            return Quaternion{
                x: Quaternion::mix_t(self.x, y.x, a),
                y: Quaternion::mix_t(self.y, y.y, a),
                z: Quaternion::mix_t(self.z, y.z, a),
                w: Quaternion::mix_t(self.w, y.w, a) };
        }
        // Essential Mathematics, page 467
        let angle = cos_theta.acos();
        return (self * ((T::one() - a) * angle).sin() + y * (a * angle).sin()) / angle.sin();
    }

    /// Linear interpolation of two quaternions.
    /// The interpolation is oriented.
    /// 
    /// _x_: A quaternion
    ///
    /// _y_: A quaternion
    ///
    /// _a_: Interpolation factor. The interpolation is defined in the range [0, 1].
    ///
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternion::new( 3.0,  4.0, 5.0, 6.0);
    /// let y = Quaternion::new( 7.0,  8.0, 9.0,10.0);
    /// let r = q.lerp(y, 0.2);
    ///
    /// assert!(eq_approx(r.x, 3.8, 1e-5));
    /// assert!(eq_approx(r.y, 4.8, 1e-5));
    /// assert!(eq_approx(r.z, 5.8, 1e-5));
    /// assert!(eq_approx(r.w, 6.8, 1e-5));
    /// ```
    pub fn lerp(self, y:Quaternion<T>, a:T) -> Quaternion<T> {
        // Lerp is only defined in [0,1]
        assert!(a >= T::zero());
        assert!(a <= T::one());

        return self * (T::one() - a) + (y * a);
    }

    /// Spherical linear interpolation of two quaternions.
    /// The interpolation always take the short path and the rotation is performed at constant speed.
    /// 
    /// _x_; A quaternion
    ///
    /// _y_: A quaternion
    ///
    /// _a_; Interpolation factor. The interpolation is defined beyond the range [0, 1].
    ///
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternion::new( 3.0,  4.0, 5.0, 6.0);
    /// let y = Quaternion::new( 7.0,  8.0, 9.0,10.0);
    /// let r = q.slerp(y, 0.2);
    ///
    /// assert!(eq_approx(r.x, 3.8, 1e-5));
    /// assert!(eq_approx(r.y, 4.8, 1e-5));
    /// assert!(eq_approx(r.z, 5.8, 1e-5));
    /// assert!(eq_approx(r.w, 6.8, 1e-5));
    /// ```
    pub fn slerp(self, y:Quaternion<T>, a:T) -> Quaternion<T> {
        let mut z = y;
        let mut cos_theta = self.dot(y);

        // If cos_theta < 0, the interpolation will take the long way around the sphere.
        // To fix this, one quaternion must be negated.
        if cos_theta < T::zero() {
            z = -y;
            cos_theta = -cos_theta;
        }

        // Perform a linear interpolation when cosTheta is close to 1 to avoid side effect of sin(angle) becoming a zero denominator
        if cos_theta > T::one() - T::one()/*epsilon<T>()*/ {
            // Linear interpolation
            return Quaternion{ x: Quaternion::mix_t(self.x, z.x, a),
                               y: Quaternion::mix_t(self.y, z.y, a),
                               z: Quaternion::mix_t(self.z, z.z, a),
                               w: Quaternion::mix_t(self.w, z.w, a) };
        }
        let angle = cos_theta.acos();
        return (self * ((T::one() - a).sin() * angle) + z * (a * angle).sin()) / angle.sin();
    }

    /// Returns the conjugate of the quaternion.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternion::new( 3.0,  4.0, 5.0, 6.0);
    /// let r = q.conjugate();
    ///
    /// assert!(r.x == -3.0);
    /// assert!(r.y == -4.0);
    /// assert!(r.z == -5.0);
    /// assert!(r.w ==  6.0);
    /// ```
    pub fn conjugate(self) -> Quaternion<T> {
        Quaternion{ x: -self.x,
                    y: -self.y,
                    z: -self.z,
                    w:  self.w }
    }

    /// Returns the inverse of the quaternion.
    /// 
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternion::new( 3.0, 4.0, 5.0, 6.0);
    /// let r = q.inverse();
    ///
    /// assert!(eq_approx(r.x, -0.034884, 1e-5));
    /// assert!(eq_approx(r.y, -0.046512, 1e-5));
    /// assert!(eq_approx(r.z, -0.05814 , 1e-5));
    /// assert!(eq_approx(r.w,  0.069767, 1e-5));
    /// ```
    pub fn inverse(self) -> Quaternion<T> {
        self.conjugate() / self.dot(self)
    }

    /// Rotates a quaternion from a vector of 3 components axis and an angle.
    /// 
    /// _self_: Source orientation
    ///
    /// _angle_: Angle expressed in radians.
    ///
    /// _axis_: Axis of the rotation
    /// 
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternion::new( 3.0, 4.0, 5.0, 6.0);
    /// let v = Vector3::new(7.0, 8.0, 9.0);
    /// let r = q.rotate(0.2, v);
    ///
    /// assert!(eq_approx(r.x, 3.257382, 1e-5));
    /// assert!(eq_approx(r.y, 4.381403, 1e-5));
    /// assert!(eq_approx(r.z, 5.333402, 1e-5));
    /// assert!(eq_approx(r.w, 5.267598, 1e-5));
    /// ```
    pub fn rotate(self, angle:T, axis:Vector3<T>) -> Quaternion<T> {
        let mut tmp = axis;

        // Axis of rotation must be normalised
        let len = tmp.length();
        if (len - T::one()).abs() > T::from(0.001).unwrap() {
            let one_over_len = T::one() / len;
            tmp.x = tmp.x * one_over_len;
            tmp.y = tmp.y * one_over_len;
            tmp.z = tmp.z * one_over_len;
        }

        let angle_rad = angle;
        let sin = (angle_rad * T::from(0.5).unwrap()).sin();

        return self * Quaternion{x: tmp.x * sin, 
                                 y: tmp.y * sin, 
                                 z: tmp.z * sin,
                                 w: (angle_rad * T::from(0.5).unwrap()).cos() };
    }

    /// Returns euler angles: pitch as x, yaw as y, roll as z.
    /// The result is expressed in radians if GLM_FORCE_RADIANS is defined or degrees otherwise.
    /// 
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternion::new( 0.6, 0.7, 0.8, 0.2);
    /// let r = q.euler_angles();
    ///
    /// assert!(eq_approx(r.x, 1.695151, 1e-5));
    /// assert!(eq_approx(r.y,-0.747763, 1e-5));
    /// assert!(eq_approx(r.z, 2.132489, 1e-5));
    /// ```
    pub fn euler_angles(self) -> Vector3<T> {
        Vector3::new(self.pitch(), self.yaw(), self.roll())
    }

    /// Returns pitch value of euler angles expressed in radians.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternion::new( 0.6, 0.7, 0.8, 0.2);
    /// let r = q.pitch();
    ///
    /// assert!(eq_approx(r, 1.695151, 1e-5));
    /// ```
    pub fn pitch(self) -> T {
        let y = T::from(2).unwrap() * (self.y * self.z + self.w * self.x);
        let x = self.w * self.w - self.x * self.x - self.y * self.y + self.z * self.z;
        return y.atan2(x);
    }

    /// Returns yaw value of euler angles expressed in radians.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternion::new( 0.6, 0.7, 0.8, 0.2);
    /// let r = q.yaw();
    ///
    /// assert!(eq_approx(r, -0.747763, 1e-5));
    /// ```
    pub fn yaw(self) -> T {
        (T::from(-2).unwrap() * (self.x * self.z - self.w * self.y)).asin()
    }

    /// Returns roll value of euler angles expressed in radians.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternion::new( 0.6, 0.7, 0.8, 0.2);
    /// let r = q.roll();
    ///
    /// assert!(eq_approx(r, 2.132489, 1e-5));
    /// ```
    pub fn roll(self) -> T {
        let y = T::from(2).unwrap() * (self.x * self.y + self.w * self.z);
        let x = self.w * self.w + self.x * self.x - self.y * self.y - self.z * self.z; 
        return y.atan2(x);
    }

    /// Converts a quaternion to a 3 * 3 matrix.
    /// 
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternion::new( 3.0, 4.0, 5.0, 6.0);
    /// let r = q.m3_cast();
    /// let e = Matrix3::new(-81.0, 84.0, -18.0,
    ///                 -36.0,-67.0,  76.0,
    ///                  78.0,  4.0, -49.0 );
    ///
    /// assert!(r.eq_approx(e, 1e-5));
    /// ```
    pub fn m3_cast(self) -> Matrix3<T> {
        let mut result = Matrix3::one();
        let qxx = self.x * self.x;
        let qyy = self.y * self.y;
        let qzz = self.z * self.z;
        let qxz = self.x * self.z;
        let qxy = self.x * self.y;
        let qyz = self.y * self.z;
        let qwx = self.w * self.x;
        let qwy = self.w * self.y;
        let qwz = self.w * self.z;
        let one = T::one();
        let two = T::from(2).unwrap();

        result[0][0] = one - two * (qyy +  qzz);
        result[0][1] = two * (qxy + qwz);
        result[0][2] = two * (qxz - qwy);

        result[1][0] = two * (qxy - qwz);
        result[1][1] = one - two * (qxx +  qzz);
        result[1][2] = two * (qyz + qwx);

        result[2][0] = two * (qxz + qwy);
        result[2][1] = two * (qyz - qwx);
        result[2][2] = one - two * (qxx +  qyy);
        return result;        
    }

    /// Converts a quaternion to a 4 * 4 matrix.
    /// 
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternion::new( 3.0, 4.0, 5.0, 6.0);
    /// let r = q.m4_cast();
    /// let e = Matrix4::new(-81.0, 84.0,-18.0, 0.0,
    ///                 -36.0,-67.0, 76.0, 0.0,
    ///                  78.0,  4.0,-49.0, 0.0,
    ///                   0.0,  0.0,  0.0, 1.0 );
    ///
    /// assert!(r.eq_approx(e, 1e-5));
    /// ```
    pub fn m4_cast(self) -> Matrix4<T> {
        Matrix4x4::new_m3x3( self.m3_cast() )
    }

    /// Returns the quaternion rotation angle.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternion::new( 0.6, 0.7, 0.8, 0.2);
    /// let r = q.angle();
    ///
    /// assert!(eq_approx(r, 2.738877, 1e-5));
    /// ```
    pub fn angle(self) -> T {
        self.w.acos() * T::from(2).unwrap()
    }

    /// Returns the q rotation axis.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let q = Quaternion::new( 0.6, 0.7, 0.8, 0.2);
    /// let r = q.axis();
    ///
    /// assert!(eq_approx(r.x, 0.612372, 1e-5));
    /// assert!(eq_approx(r.y, 0.714435, 1e-5));
    /// assert!(eq_approx(r.z, 0.816497, 1e-5));
    /// ```
    pub fn axis(self) -> Vector3<T> {
        let tmp1 = T::one() - self.w * self.w;
        if tmp1 <= T::zero() {
            return Vector3::new(T::zero(), T::zero(), T::one());
        }
        let tmp2 = T::one() / tmp1.sqrt();
        return Vector3::new(self.x * tmp2, self.y * tmp2, self.z * tmp2);        
    }

    /// Returns the component-wise comparison result of x < y.
    /// 
    /// ```
    /// use gml::*;
    ///
    /// let a = Quaternion::new( 3.0, 4.0, 5.0, 6.0);
    /// let b = Quaternion::new( 2.0, 4.0, 9.0,-4.0);
    /// let r = a.lt(b);
    ///
    /// assert!(r.x == (a.x < b.x));
    /// assert!(r.y == (a.y < b.y));
    /// assert!(r.z == (a.z < b.z));
    /// assert!(r.w == (a.w < b.w));
    /// ```
    pub fn lt(self, y:Quaternion<T>) -> Vector4b {
        let mut result = Vector4b::new( false, false, false, false );
        for i in 0..4 {
            result[i] = self[i] < y[i];
        }
        return result;
    }

    /// Returns the component-wise comparison of result x <= y.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let a = Quaternion::new( 3.0, 4.0, 5.0, 6.0);
    /// let b = Quaternion::new( 2.0, 4.0, 9.0,-4.0);
    /// let r = a.le(b);
    ///
    /// assert!(r.x == (a.x <= b.x));
    /// assert!(r.y == (a.y <= b.y));
    /// assert!(r.z == (a.z <= b.z));
    /// assert!(r.w == (a.w <= b.w));
    /// ```
    pub fn le(self, y:Quaternion<T>) -> Vector4b {
        let mut result = Vector4b::new( false, false, false, false );
        for i in 0..4 {
            result[i] = self[i] <= y[i];
        }
        return result;
    }
    
    /// Returns the component-wise comparison of result x > y.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let a = Quaternion::new( 3.0, 4.0, 5.0, 6.0);
    /// let b = Quaternion::new( 2.0, 4.0, 9.0,-4.0);
    /// let r = a.gt(b);
    ///
    /// assert!(r.x == (a.x > b.x));
    /// assert!(r.y == (a.y > b.y));
    /// assert!(r.z == (a.z > b.z));
    /// assert!(r.w == (a.w > b.w));
    /// ```
    pub fn gt(self, y:Quaternion<T>) -> Vector4b {
        let mut result = Vector4b::new( false, false, false, false );
        for i in 0..4 {
            result[i] = self[i] > y[i];
        }
        return result;
    }

    /// Returns the component-wise comparison of result x >= y.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let a = Quaternion::new( 3.0, 4.0, 5.0, 6.0);
    /// let b = Quaternion::new( 2.0, 4.0, 9.0,-4.0);
    /// let r = a.ge(b);
    ///
    /// assert!(r.x == (a.x >= b.x));
    /// assert!(r.y == (a.y >= b.y));
    /// assert!(r.z == (a.z >= b.z));
    /// assert!(r.w == (a.w >= b.w));
    /// ```
    pub fn ge(self, y:Quaternion<T>) -> Vector4b {
        let mut result = Vector4b::new( false, false, false, false );
        for i in 0..4 {
            result[i] = self[i] >= y[i];
        }
        return result;
    }

    /// Returns the component-wise comparison of result x == y.
    ///
    /// ```
    /// use gml::*;
    ///
    /// let a = Quaternion::new( 3.0, 4.0, 5.0, 6.0);
    /// let b = Quaternion::new( 2.0, 4.0, 9.0,-4.0);
    /// let r = a.eq(b);
    ///
    /// assert!(r.x == (a.x == b.x));
    /// assert!(r.y == (a.y == b.y));
    /// assert!(r.z == (a.z == b.z));
    /// assert!(r.w == (a.w == b.w));
    /// ```
    pub fn eq(self, y:Quaternion<T>) -> Vector4b {
        let mut result = Vector4b::new( false, false, false, false );
        for i in 0..4 {
            result[i] = self[i] == y[i];
        }
        return result;
    }

    /// Returns the component-wise comparison of result x != y.
    /// 
    /// ```
    /// use gml::*;
    ///
    /// let a = Quaternion::new( 3.0, 4.0, 5.0, 6.0);
    /// let b = Quaternion::new( 2.0, 4.0, 9.0,-4.0);
    /// let r = a.ne(b);
    ///
    /// assert!(r.x == (a.x != b.x));
    /// assert!(r.y == (a.y != b.y));
    /// assert!(r.z == (a.z != b.z));
    /// assert!(r.w == (a.w != b.w));
    /// ```
    pub fn ne(self, y:Quaternion<T>) -> Vector4b {
        let mut result = Vector4b::new( false, false, false, false );
        for i in 0..4 {
            result[i] = self[i] != y[i];
        }
        return result;
    }

    
}