vox_geometry_rust 0.1.2

/*
 * // Copyright (c) 2021 Feng Yang
 * //
 * // I am making my contributions/submissions to this project solely in my
 * // personal capacity and am not conveying any rights to any intellectual
 * // property of any third parties.
 */

use num::Float;
use crate::vector3::Vector3;
use crate::matrix3x3::Matrix3x3;
use crate::matrix4x4::Matrix4x4;
use crate::constants::pi;
use std::ops::{IndexMut, Index, Mul, MulAssign};
use std::fmt::{Debug, Formatter, Result};

///
/// # Quaternion class defined as q = w + xi + yj + zk.
///
pub struct Quaternion<T: Float> {
    /// Real part.
    pub w: T,

    ///< Imaginary part (i).
    pub x: T,

    ///< Imaginary part (j).
    pub y: T,

    ///< Imaginary part (k).
    pub z: T,
}

/// Float-type quaternion.
pub type QuaternionF = Quaternion<f32>;

/// Double-type quaternion.
pub type QuaternionD = Quaternion<f64>;

/// # Constructors
impl<T: Float> Quaternion<T> {
    /// Make an identity quaternion.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// let q = QuaternionD::new_default();
    ///
    /// assert_eq!(1.0, q.w);
    /// assert_eq!(0.0, q.x);
    /// assert_eq!(0.0, q.y);
    /// assert_eq!(0.0, q.z);
    /// ```
    pub fn new_default() -> Quaternion<T> {
        return Quaternion {
            w: T::one(),
            x: T::zero(),
            y: T::zero(),
            z: T::zero(),
        };
    }

    /// Constructs a quaternion with given elements.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// let q = QuaternionD::new(1.0, 2.0, 3.0, 4.0);
    ///
    /// assert_eq!(1.0, q.w);
    /// assert_eq!(2.0, q.x);
    /// assert_eq!(3.0, q.y);
    /// assert_eq!(4.0, q.z);
    /// ```
    pub fn new(new_w: T, new_x: T, new_y: T, new_z: T) -> Quaternion<T> {
        return Quaternion {
            w: new_w,
            x: new_x,
            y: new_y,
            z: new_z,
        };
    }

    /// Constructs a quaternion with given elements.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// let q = QuaternionD::new_lst([1.0, 2.0, 3.0, 4.0]);
    ///
    /// assert_eq!(1.0, q.w);
    /// assert_eq!(2.0, q.x);
    /// assert_eq!(3.0, q.y);
    /// assert_eq!(4.0, q.z);
    /// ```
    pub fn new_lst(lst: [T; 4]) -> Quaternion<T> {
        return Quaternion {
            w: lst[0],
            x: lst[1],
            y: lst[2],
            z: lst[3],
        };
    }

    /// Constructs a quaternion with given rotation axis and angle.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// use vox_geometry_rust::vector3::Vector3D;
    /// use vox_geometry_rust::assert_delta;
    /// let original_axis = Vector3D::new(1.0, 3.0, 2.0).normalized();
    /// let original_angle = 0.4;
    ///
    /// let q = QuaternionD::new_axis(original_axis, original_angle);
    ///
    /// let axis = q.axis();
    /// let angle = q.angle();
    ///
    /// assert_delta!(original_axis.x, axis.x, f64::EPSILON);
    /// assert_delta!(original_axis.y, axis.y, f64::EPSILON);
    /// assert_delta!(original_axis.z, axis.z, f64::EPSILON);
    /// assert_delta!(original_angle, angle, f64::EPSILON);
    /// ```
    pub fn new_axis(axis: Vector3<T>, angle: T) -> Quaternion<T> {
        let eps = T::epsilon();

        let axis_length_squared = axis.length_squared();

        return if axis_length_squared < eps {
            Quaternion {
                w: T::one(),
                x: T::zero(),
                y: T::zero(),
                z: T::zero(),
            }
        } else {
            let normalized_axis = axis.normalized();
            let s = T::sin(angle / T::from(2.0).unwrap());

            Quaternion {
                w: T::cos(angle / T::from(2.0).unwrap()),
                x: normalized_axis.x * s,
                y: normalized_axis.y * s,
                z: normalized_axis.z * s,
            }
        };
    }

    /// Constructs a quaternion with from and to vectors.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// use vox_geometry_rust::vector3::Vector3D;
    /// use vox_geometry_rust::constants::pi;
    /// use vox_geometry_rust::assert_delta;
    /// let from = Vector3D::new(1.0, 0.0, 0.0);
    /// let to = Vector3D::new(0.0, 0.0, 1.0);
    ///
    /// let q = QuaternionD::new_from(from, to);
    ///
    /// let axis = q.axis();
    /// let angle = q.angle();
    ///
    /// assert_eq!(0.0, axis.x);
    /// assert_eq!(-1.0, axis.y);
    /// assert_eq!(0.0, axis.z);
    /// assert_delta!(pi::<f64>()/2.0, angle, f64::EPSILON);
    /// ```
    pub fn new_from(from: Vector3<T>, to: Vector3<T>) -> Quaternion<T> {
        let eps = T::epsilon();

        let mut axis = from.cross(&to);

        let from_length_squared = from.length_squared();
        let to_length_squared = to.length_squared();

        return if from_length_squared < eps ||
            to_length_squared < eps {
            Quaternion {
                w: T::one(),
                x: T::zero(),
                y: T::zero(),
                z: T::zero(),
            }
        } else {
            let axis_length_squared = axis.length_squared();

            // In case two vectors are exactly the opposite, pick orthogonal vector
            // for axis.
            if axis_length_squared < eps {
                axis = from.tangential().0;
            }

            let mut result = Quaternion::new(from.dot(&to), axis.x, axis.y, axis.z);
            result.w = result.w + result.l2norm();
            result.normalize();
            result
        };
    }

    /// Constructs a quaternion with three basis vectors.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// use vox_geometry_rust::vector3::Vector3D;
    /// use vox_geometry_rust::assert_delta;
    /// let rotation_basis0 = Vector3D::new(1.0, 0.0, 0.0);
    /// let rotation_basis1 = Vector3D::new(0.0, 0.0, 1.0);
    /// let rotation_basis2 = Vector3D::new(0.0, -1.0, 0.0);
    ///
    /// let q = QuaternionD::new_basis(rotation_basis0, rotation_basis1, rotation_basis2);
    ///
    /// assert_delta!(f64::sqrt(2.0) / 2.0, q.w, f64::EPSILON);
    /// assert_delta!(f64::sqrt(2.0) / 2.0, q.x, f64::EPSILON);
    /// assert_eq!(0.0, q.y);
    /// assert_eq!(0.0, q.z);
    /// ```
    pub fn new_basis(rotation_basis0: Vector3<T>,
                     rotation_basis1: Vector3<T>,
                     rotation_basis2: Vector3<T>) -> Quaternion<T> {
        let mut matrix3 = Matrix3x3::new_default();

        matrix3.set_column(0, rotation_basis0.normalized());
        matrix3.set_column(1, rotation_basis1.normalized());
        matrix3.set_column(2, rotation_basis2.normalized());
        return Quaternion::new_mat(matrix3);
    }

    /// Constructs a quaternion with 3x3 rotational matrix.
    pub fn new_mat(m: Matrix3x3<T>) -> Quaternion<T> {
        let eps = T::epsilon();
        let quater = T::from(0.25).unwrap();

        let one_plus_trace = m.trace() + T::one();

        if one_plus_trace > eps {
            let s = T::sqrt(one_plus_trace) * T::from(2.0).unwrap();
            return Quaternion {
                w: quater * s,
                x: (m[(2, 1)] - m[(1, 2)]) / s,
                y: (m[(0, 2)] - m[(2, 0)]) / s,
                z: (m[(1, 0)] - m[(0, 1)]) / s,
            };
        } else if m[(0, 0)] > m[(1, 1)] && m[(0, 0)] > m[(2, 2)] {
            let s = T::sqrt(T::one() + m[(0, 0)] - m[(1, 1)] - m[(2, 2)]) * T::from(2.0).unwrap();
            return Quaternion {
                w: (m[(2, 1)] - m[(1, 2)]) / s,
                x: quater * s,
                y: (m[(0, 1)] + m[(1, 0)]) / s,
                z: (m[(0, 2)] + m[(2, 0)]) / s,
            };
        } else if m[(1, 1)] > m[(2, 2)] {
            let s = T::sqrt(T::one() + m[(1, 1)] - m[(0, 0)] - m[(2, 2)]) * T::from(2.0).unwrap();
            return Quaternion {
                w: (m[(0, 2)] - m[(2, 0)]) / s,
                x: (m[(0, 1)] + m[(1, 0)]) / s,
                y: quater * s,
                z: (m[(1, 2)] + m[(2, 1)]) / s,
            };
        } else {
            let s = T::sqrt(T::one() + m[(2, 2)] - m[(0, 0)] - m[(1, 1)]) * T::from(2.0).unwrap();
            return Quaternion {
                w: (m[(1, 0)] - m[(0, 1)]) / s,
                x: (m[(0, 2)] + m[(2, 0)]) / s,
                y: (m[(1, 2)] + m[(2, 1)]) / s,
                z: quater * s,
            };
        }
    }
}

/// # Basic setters
impl<T: Float> Quaternion<T> {
    /// Sets the quaternion with other quaternion.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// let mut q = QuaternionD::new_default();
    /// q.set_self(QuaternionD::new(1.0, 2.0, 3.0, 4.0));
    ///
    /// assert_eq!(1.0, q.w);
    /// assert_eq!(2.0, q.x);
    /// assert_eq!(3.0, q.y);
    /// assert_eq!(4.0, q.z);
    /// ```
    pub fn set_self(&mut self, other: Quaternion<T>) {
        self.set_scalar(other.w, other.x, other.y, other.z);
    }

    /// Sets the quaternion with given elements.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// let mut q = QuaternionD::new_default();
    /// q.set_scalar(1.0, 2.0, 3.0, 4.0);
    ///
    /// assert_eq!(1.0, q.w);
    /// assert_eq!(2.0, q.x);
    /// assert_eq!(3.0, q.y);
    /// assert_eq!(4.0, q.z);
    /// ```
    pub fn set_scalar(&mut self, new_w: T, new_x: T, new_y: T, new_z: T) {
        self.w = new_w;
        self.x = new_x;
        self.y = new_y;
        self.z = new_z;
    }

    /// Sets the quaternion with given elements.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// let mut q = QuaternionD::new_default();
    /// q.set_lst([1.0, 2.0, 3.0, 4.0 ]);
    ///
    /// assert_eq!(1.0, q.w);
    /// assert_eq!(2.0, q.x);
    /// assert_eq!(3.0, q.y);
    /// assert_eq!(4.0, q.z);
    /// ```
    pub fn set_lst(&mut self, lst: [T; 4]) {
        self.w = lst[0];
        self.x = lst[1];
        self.y = lst[2];
        self.z = lst[3];
    }

    /// Sets the quaternion with given rotation axis and angle.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// use vox_geometry_rust::vector3::Vector3D;
    /// use vox_geometry_rust::assert_delta;
    /// let original_axis = Vector3D::new(1.0, 3.0, 2.0).normalized();
    /// let original_angle = 0.4;
    ///
    /// let mut q = QuaternionD::new_default();
    /// q.set_axis(original_axis, original_angle);
    ///
    /// let axis = q.axis();
    /// let angle = q.angle();
    ///
    /// assert_delta!(original_axis.x, axis.x, f64::EPSILON);
    /// assert_delta!(original_axis.y, axis.y, f64::EPSILON);
    /// assert_delta!(original_axis.z, axis.z, f64::EPSILON);
    /// assert_delta!(original_angle, angle, f64::EPSILON);
    /// ```
    pub fn set_axis(&mut self, axis: Vector3<T>, angle: T) {
        let eps = T::epsilon();

        let axis_length_squared = axis.length_squared();

        if axis_length_squared < eps {
            self.set_identity();
        } else {
            let normalized_axis = axis.normalized();
            let s = T::sin(angle / T::from(2.0).unwrap());

            self.x = normalized_axis.x * s;
            self.y = normalized_axis.y * s;
            self.z = normalized_axis.z * s;
            self.w = T::cos(angle / T::from(2.0).unwrap());
        }
    }

    /// Sets the quaternion with from and to vectors.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// use vox_geometry_rust::vector3::Vector3D;
    /// use vox_geometry_rust::constants::pi;
    /// use vox_geometry_rust::assert_delta;
    /// let from = Vector3D::new(1.0, 0.0, 0.0);
    /// let to = Vector3D::new(0.0, 0.0, 1.0);
    ///
    /// let mut q = QuaternionD::new_default();
    /// q.set_from(from, to);
    ///
    /// let axis = q.axis();
    /// let angle = q.angle();
    ///
    /// assert_eq!(0.0, axis.x);
    /// assert_eq!(-1.0, axis.y);
    /// assert_eq!(0.0, axis.z);
    /// assert_delta!(pi::<f64>()/2.0, angle, f64::EPSILON);
    /// ```
    pub fn set_from(&mut self, from: Vector3<T>, to: Vector3<T>) {
        let eps = T::epsilon();

        let mut axis = from.cross(&to);

        let from_length_squared = from.length_squared();
        let to_length_squared = to.length_squared();

        if from_length_squared < eps ||
            to_length_squared < eps {
            self.set_identity();
        } else {
            let axis_length_squared = axis.length_squared();

            // In case two vectors are exactly the opposite, pick orthogonal vector
            // for axis.
            if axis_length_squared < eps {
                axis = from.tangential().0;
            }

            self.set_scalar(from.dot(&to), axis.x, axis.y, axis.z);
            self.w = self.w + self.l2norm();

            self.normalize();
        }
    }

    /// Sets quaternion with three basis vectors.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// use vox_geometry_rust::vector3::Vector3D;
    /// use vox_geometry_rust::assert_delta;
    /// let rotation_basis0 = Vector3D::new(1.0, 0.0, 0.0);
    /// let rotation_basis1 = Vector3D::new(0.0, 0.0, 1.0);
    /// let rotation_basis2 = Vector3D::new(0.0, -1.0, 0.0);
    ///
    /// let mut q = QuaternionD::new_default();
    /// q.set_basis(rotation_basis0, rotation_basis1, rotation_basis2);
    ///
    /// assert_delta!(f64::sqrt(2.0) / 2.0, q.w, f64::EPSILON);
    /// assert_delta!(f64::sqrt(2.0) / 2.0, q.x, f64::EPSILON);
    /// assert_eq!(0.0, q.y);
    /// assert_eq!(0.0, q.z);
    /// ```
    pub fn set_basis(&mut self,
                     rotation_basis0: Vector3<T>,
                     rotation_basis1: Vector3<T>,
                     rotation_basis2: Vector3<T>) {
        let mut matrix3 = Matrix3x3::new_default();

        matrix3.set_column(0, rotation_basis0.normalized());
        matrix3.set_column(1, rotation_basis1.normalized());
        matrix3.set_column(2, rotation_basis2.normalized());

        self.set_mat(matrix3);
    }

    /// Sets the quaternion with 3x3 rotational matrix.
    pub fn set_mat(&mut self, m: Matrix3x3<T>) {
        let eps = T::epsilon();
        let quater = T::from(0.25).unwrap();

        let one_plus_trace = m.trace() + T::one();

        if one_plus_trace > eps {
            let s = T::sqrt(one_plus_trace) * T::from(2.0).unwrap();
            self.w = quater * s;
            self.x = (m[(2, 1)] - m[(1, 2)]) / s;
            self.y = (m[(0, 2)] - m[(2, 0)]) / s;
            self.z = (m[(1, 0)] - m[(0, 1)]) / s;
        } else if m[(0, 0)] > m[(1, 1)] && m[(0, 0)] > m[(2, 2)] {
            let s = T::sqrt(T::one() + m[(0, 0)] - m[(1, 1)] - m[(2, 2)]) * T::from(2.0).unwrap();
            self.w = (m[(2, 1)] - m[(1, 2)]) / s;
            self.x = quater * s;
            self.y = (m[(0, 1)] + m[(1, 0)]) / s;
            self.z = (m[(0, 2)] + m[(2, 0)]) / s;
        } else if m[(1, 1)] > m[(2, 2)] {
            let s = T::sqrt(T::one() + m[(1, 1)] - m[(0, 0)] - m[(2, 2)]) * T::from(2.0).unwrap();
            self.w = (m[(0, 2)] - m[(2, 0)]) / s;
            self.x = (m[(0, 1)] + m[(1, 0)]) / s;
            self.y = quater * s;
            self.z = (m[(1, 2)] + m[(2, 1)]) / s;
        } else {
            let s = T::sqrt(T::one() + m[(2, 2)] - m[(0, 0)] - m[(1, 1)]) * T::from(2.0).unwrap();
            self.w = (m[(1, 0)] - m[(0, 1)]) / s;
            self.x = (m[(0, 2)] + m[(2, 0)]) / s;
            self.y = (m[(1, 2)] + m[(2, 1)]) / s;
            self.z = quater * s;
        }
    }
}

/// # Basic getters
impl<T: Float> Quaternion<T> {
    /// Returns normalized quaternion.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// let q = QuaternionD::new(1.0, 2.0, 3.0, 4.0);
    /// let qn = q.normalized();
    ///
    /// let denom = f64::sqrt(30.0);
    /// assert_eq!(1.0 / denom, qn.w);
    /// assert_eq!(2.0 / denom, qn.x);
    /// assert_eq!(3.0 / denom, qn.y);
    /// assert_eq!(4.0 / denom, qn.z);
    /// ```
    pub fn normalized(&self) -> Quaternion<T> {
        let mut q = self.clone();
        q.normalize();
        return q;
    }
}

/// # Binary operator methods - new instance = this instance (+) input
impl<T: Float> Quaternion<T> {
    /// Returns this quaternion * vector.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// use vox_geometry_rust::vector3::Vector3D;
    /// let mut q1 = QuaternionD::new(1.0, 2.0, 3.0, 4.0);
    ///
    /// q1.normalize();
    /// let v = Vector3D::new(7.0, 8.0, 9.0);
    /// let ans1 = q1.mul_vec(v);
    ///
    /// let m = q1.matrix3();
    /// let ans2 = m.mul_vec(v);
    ///
    /// assert_eq!(ans2.x, ans1.x);
    /// assert_eq!(ans2.y, ans1.y);
    /// assert_eq!(ans2.z, ans1.z);
    /// ```
    pub fn mul_vec(&self, v: Vector3<T>) -> Vector3<T> {
        let _2xx = T::from(2.0).unwrap() * self.x * self.x;
        let _2yy = T::from(2.0).unwrap() * self.y * self.y;
        let _2zz = T::from(2.0).unwrap() * self.z * self.z;
        let _2xy = T::from(2.0).unwrap() * self.x * self.y;
        let _2xz = T::from(2.0).unwrap() * self.x * self.z;
        let _2xw = T::from(2.0).unwrap() * self.x * self.w;
        let _2yz = T::from(2.0).unwrap() * self.y * self.z;
        let _2yw = T::from(2.0).unwrap() * self.y * self.w;
        let _2zw = T::from(2.0).unwrap() * self.z * self.w;

        return Vector3::new(
            (T::one() - _2yy - _2zz) * v.x + (_2xy - _2zw) * v.y + (_2xz + _2yw) * v.z,
            (_2xy + _2zw) * v.x + (T::one() - _2zz - _2xx) * v.y + (_2yz - _2xw) * v.z,
            (_2xz - _2yw) * v.x + (_2yz + _2xw) * v.y + (T::one() - _2yy - _2xx) * v.z);
    }

    /// Returns this quaternion * other quaternion.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// let q1 = QuaternionD::new(1.0, 2.0, 3.0, 4.0);
    /// let q2 = QuaternionD::new(1.0, -2.0, -3.0, -4.0);
    ///
    /// let q3 = q1.mul_quat(q2);
    ///
    /// assert_eq!(30.0, q3.w);
    /// assert_eq!(0.0, q3.x);
    /// assert_eq!(0.0, q3.y);
    /// assert_eq!(0.0, q3.z);
    /// ```
    pub fn mul_quat(&self, other: Quaternion<T>) -> Quaternion<T> {
        return Quaternion::new(
            self.w * other.w - self.x * other.x - self.y * other.y - self.z * other.z,
            self.w * other.x + self.x * other.w + self.y * other.z - self.z * other.y,
            self.w * other.y - self.x * other.z + self.y * other.w + self.z * other.x,
            self.w * other.z + self.x * other.y - self.y * other.x + self.z * other.w);
    }

    /// Computes the dot product with other quaternion.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// let q1 = QuaternionD::new(1.0, 2.0, 3.0, 4.0);
    /// let q2 = QuaternionD::new(5.0, 6.0, 7.0, 8.0);
    /// assert_eq!(70.0, q1.dot(q2));
    /// ```
    pub fn dot(&self, other: Quaternion<T>) -> T {
        return self.w * other.w + self.x * other.x + self.y * other.y + self.z * other.z;
    }
}

/// # Binary operator methods - new instance = input (+) this instance
impl<T: Float> Quaternion<T> {
    /// Returns other quaternion * this quaternion.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// let q1 = QuaternionD::new(1.0, 2.0, 3.0, 4.0);
    /// let q2 = QuaternionD::new(5.0, 6.0, 7.0, 8.0);
    /// let q3 = q1.mul_quat(q2);
    /// assert_eq!(q3, q2.rmul_quat(q1));
    /// ```
    pub fn rmul_quat(&self, other: Quaternion<T>) -> Quaternion<T> {
        return Quaternion::new(
            other.w * self.w - other.x * self.x - other.y * self.y - other.z * self.z,
            other.w * self.x + other.x * self.w + other.y * self.z - other.z * self.y,
            other.w * self.y - other.x * self.z + other.y * self.w + other.z * self.x,
            other.w * self.z + other.x * self.y - other.y * self.x + other.z * self.w);
    }
}

/// # Augmented operator methods - this instance (+)= input
impl<T: Float> Quaternion<T> {
    /// Returns this quaternion *= other quaternion.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// let mut q1 = QuaternionD::new(1.0, 2.0, 3.0, 4.0);
    /// let q2 = QuaternionD::new(5.0, 6.0, 7.0, 8.0);
    /// let q3 = q1.mul_quat(q2);
    /// q1.imul_quat(q2);
    /// assert_eq!(q3, q1);
    /// ```
    pub fn imul_quat(&mut self, other: Quaternion<T>) {
        *self = self.mul_quat(other);
    }
}

/// # Modifiers
impl<T: Float> Quaternion<T> {
    /// Makes this quaternion identity.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// let mut q = QuaternionD::new(4.0, 3.0, 2.0, 1.0);
    /// q.set_identity();
    ///
    /// assert_eq!(1.0, q.w);
    /// assert_eq!(0.0, q.x);
    /// assert_eq!(0.0, q.y);
    /// assert_eq!(0.0, q.z);
    /// ```
    pub fn set_identity(&mut self) {
        self.set_scalar(T::one(), T::zero(), T::zero(), T::zero());
    }

    /// Rotate this quaternion with given angle in radians.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// use vox_geometry_rust::vector3::Vector3D;
    /// let mut q = QuaternionD::new(4.0, 3.0, 2.0, 1.0);
    /// q.normalize();
    ///
    /// let mut axis = Vector3D::new_default();
    /// let mut angle = 0.0;
    /// q.get_axis_angle(&mut axis, &mut angle);
    /// q.rotate(1.0);
    /// let mut new_angle = 0.0;
    /// q.get_axis_angle(&mut axis, &mut new_angle);
    ///
    /// assert_eq!(angle + 1.0, new_angle);
    /// ```
    pub fn rotate(&mut self, angle_in_radians: T) {
        let mut axis = Vector3::new_default();
        let mut current_angle = T::zero();

        self.get_axis_angle(&mut axis, &mut current_angle);

        current_angle = current_angle + angle_in_radians;

        self.set_axis(axis, current_angle);
    }

    /// Normalizes the quaternion.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// let mut q = QuaternionD::new(4.0, 3.0, 2.0, 1.0);
    /// q.normalize();
    ///
    /// let denom = f64::sqrt(30.0);
    /// assert_eq!(4.0 / denom, q.w);
    /// assert_eq!(3.0 / denom, q.x);
    /// assert_eq!(2.0 / denom, q.y);
    /// assert_eq!(1.0 / denom, q.z);
    /// ```
    pub fn normalize(&mut self) {
        let norm = self.l2norm();

        if norm > T::zero() {
            self.w = self.w / norm;
            self.x = self.x / norm;
            self.y = self.y / norm;
            self.z = self.z / norm;
        }
    }
}

/// # Complex getters
impl<T: Float> Quaternion<T> {
    /// Returns the rotational axis.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// use vox_geometry_rust::vector3::Vector3D;
    /// let mut q = QuaternionD::new(1.0, 0.0, 5.0, 2.0);
    ///
    /// q.normalize();
    /// assert_eq!(0.0, q.axis().x);
    /// assert_eq!(5.0 / f64::sqrt(29.0), q.axis().y);
    /// assert_eq!(2.0 / f64::sqrt(29.0), q.axis().z);
    /// ```
    pub fn axis(&self) -> Vector3<T> {
        let mut result = Vector3::new(self.x, self.y, self.z);
        result.normalize();

        return if T::from(2.0).unwrap() * T::acos(self.w) < pi() {
            result
        } else {
            -result
        };
    }

    /// Returns the rotational angle.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// use vox_geometry_rust::vector3::Vector3D;
    /// let mut q = QuaternionD::new(1.0, 0.0, 5.0, 2.0);
    ///
    /// q.normalize();
    /// assert_eq!(2.0 * f64::acos(1.0 / f64::sqrt(30.0)), q.angle());
    /// ```
    pub fn angle(&self) -> T {
        let result = T::from(2.0).unwrap() * T::acos(self.w);

        return if result < pi() {
            result
        } else {
            // Wrap around
            T::from(2.0).unwrap() * pi() - result
        };
    }

    /// Returns the axis and angle.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// use vox_geometry_rust::vector3::Vector3D;
    /// let mut q = QuaternionD::new(1.0, 0.0, 5.0, 2.0);
    ///
    /// q.normalize();
    /// let mut axis = Vector3D::new_default();
    /// let mut angle = 0.0;
    /// q.get_axis_angle(&mut axis, &mut angle);
    ///
    /// assert_eq!(0.0, axis.x);
    /// assert_eq!(5.0 / f64::sqrt(29.0), axis.y);
    /// assert_eq!(2.0 / f64::sqrt(29.0), axis.z);
    /// ```
    pub fn get_axis_angle(&self, axis: &mut Vector3<T>, angle: &mut T) {
        axis.set_scalar3(self.x, self.y, self.z);
        axis.normalize();
        *angle = T::from(2.0).unwrap() * T::acos(self.w);

        if *angle > crate::constants::pi::<T>() {
            // Wrap around
            *axis = -*axis;
            *angle = T::from(2.0).unwrap() * pi() - (*angle);
        }
    }

    /// Returns the inverse quaternion.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// let q = QuaternionD::new(1.0, 2.0, 3.0, 4.0);
    ///
    /// let q2 = q.inverse();
    /// assert_eq!(1.0/30.0, q2.w);
    /// assert_eq!(-1.0/15.0, q2.x);
    /// assert_eq!(-1.0/10.0, q2.y);
    /// assert_eq!(-2.0/15.0, q2.z);
    /// ```
    pub fn inverse(&self) -> Quaternion<T> {
        let denom = self.w * self.w + self.x * self.x + self.y * self.y + self.z * self.z;
        return Quaternion::new(self.w / denom, -self.x / denom, -self.y / denom, -self.z / denom);
    }

    /// Converts to the 3x3 rotation matrix.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// use vox_geometry_rust::assert_delta;
    /// let mut q = QuaternionD::new(1.0, 0.0, 5.0, 2.0);
    ///
    /// q.normalize();
    /// let mat3 = q.matrix3();
    /// let solution3 = [
    ///         -14.0 / 15.0, -2.0 / 15.0, 1.0 / 3.0,
    ///         2.0 / 15.0, 11.0 / 15.0, 2.0 / 3.0,
    ///         -1.0 / 3.0, 2.0 / 3.0, -2.0 / 3.0
    ///     ];
    /// for i in 0..9 {
    ///     assert_delta!(solution3[i], mat3[i], f64::EPSILON);
    /// }
    /// ```
    pub fn matrix3(&self) -> Matrix3x3<T> {
        let _2xx = T::from(2.0).unwrap() * self.x * self.x;
        let _2yy = T::from(2.0).unwrap() * self.y * self.y;
        let _2zz = T::from(2.0).unwrap() * self.z * self.z;
        let _2xy = T::from(2.0).unwrap() * self.x * self.y;
        let _2xz = T::from(2.0).unwrap() * self.x * self.z;
        let _2xw = T::from(2.0).unwrap() * self.x * self.w;
        let _2yz = T::from(2.0).unwrap() * self.y * self.z;
        let _2yw = T::from(2.0).unwrap() * self.y * self.w;
        let _2zw = T::from(2.0).unwrap() * self.z * self.w;

        return Matrix3x3::new(
            T::one() - _2yy - _2zz, _2xy - _2zw, _2xz + _2yw,
            _2xy + _2zw, T::one() - _2zz - _2xx, _2yz - _2xw,
            _2xz - _2yw, _2yz + _2xw, T::one() - _2yy - _2xx);
    }

    /// Converts to the 4x4 rotation matrix.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// use vox_geometry_rust::assert_delta;
    /// let mut q = QuaternionD::new(1.0, 0.0, 5.0, 2.0);
    ///
    /// q.normalize();
    /// let mat4 = q.matrix4();
    /// let solution4 = [
    ///         -14.0 / 15.0, -2.0 / 15.0, 1.0 / 3.0, 0.0,
    ///         2.0 / 15.0, 11.0 / 15.0, 2.0 / 3.0, 0.0,
    ///         -1.0 / 3.0, 2.0 / 3.0, -2.0 / 3.0, 0.0,
    ///         0.0, 0.0, 0.0, 1.0
    ///     ];
    /// for i in 0..16 {
    ///     assert_delta!(solution4[i], mat4[i], f64::EPSILON);
    /// }
    /// ```
    pub fn matrix4(&self) -> Matrix4x4<T> {
        let _2xx = T::from(2.0).unwrap() * self.x * self.x;
        let _2yy = T::from(2.0).unwrap() * self.y * self.y;
        let _2zz = T::from(2.0).unwrap() * self.z * self.z;
        let _2xy = T::from(2.0).unwrap() * self.x * self.y;
        let _2xz = T::from(2.0).unwrap() * self.x * self.z;
        let _2xw = T::from(2.0).unwrap() * self.x * self.w;
        let _2yz = T::from(2.0).unwrap() * self.y * self.z;
        let _2yw = T::from(2.0).unwrap() * self.y * self.w;
        let _2zw = T::from(2.0).unwrap() * self.z * self.w;

        return Matrix4x4::new(
            T::one() - _2yy - _2zz, _2xy - _2zw, _2xz + _2yw, T::zero(),
            _2xy + _2zw, T::one() - _2zz - _2xx, _2yz - _2xw, T::zero(),
            _2xz - _2yw, _2yz + _2xw, T::one() - _2yy - _2xx, T::zero(),
            T::zero(), T::zero(), T::zero(), T::one());
    }

    /// Returns L2 norm of this quaternion.
    /// ```
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// let mut q = QuaternionD::new(1.0, 2.0, 3.0, 4.0);
    ///
    /// assert_eq!(f64::sqrt(30.0), q.l2norm());
    /// ```
    pub fn l2norm(&self) -> T {
        return T::sqrt(self.w * self.w + self.x * self.x + self.y * self.y + self.z * self.z);
    }
}

/// # Setter operators
/// Copy constructor.
/// ```
/// use vox_geometry_rust::quaternion::QuaternionD;
/// let q = QuaternionD::new(1.0, 2.0, 3.0, 4.0);
/// let mut q2 = QuaternionD::new(5.0, 6.0, 7.0, 8.0);
///
/// q2 = q;
/// assert_eq!(1.0, q2.w);
/// assert_eq!(2.0, q2.x);
/// assert_eq!(3.0, q2.y);
/// assert_eq!(4.0, q2.z);
/// ```
impl<T: Float> Clone for Quaternion<T> {
    fn clone(&self) -> Self {
        return Quaternion {
            w: self.w,
            x: self.x,
            y: self.y,
            z: self.z,
        };
    }
}

impl<T: Float> Copy for Quaternion<T> {}


/// # Getter operators
/// Returns const reference to the **i** -th element of the vector.
/// ```
/// use vox_geometry_rust::quaternion::QuaternionD;
/// let q = QuaternionD::new(1.0, 2.0, 3.0, 4.0);
///
/// assert_eq!(1.0, q[0]);
/// assert_eq!(2.0, q[1]);
/// assert_eq!(3.0, q[2]);
/// assert_eq!(4.0, q[3]);
/// ```
impl<T: Float> Index<usize> for Quaternion<T> {
    type Output = T;
    fn index(&self, index: usize) -> &Self::Output {
        return match index {
            0 => &self.w,
            1 => &self.x,
            2 => &self.y,
            3 => &self.z,
            _ => {
                panic!();
            }
        };
    }
}

/// Returns reference to the **i** -th element of the vector.
/// ```
/// use vox_geometry_rust::quaternion::QuaternionD;
/// let mut q = QuaternionD::new(1.0, 2.0, 3.0, 4.0);
/// q[0] = 5.0;
/// q[1] = 6.0;
/// q[2] = 7.0;
/// q[3] = 8.0;
///
/// assert_eq!(5.0, q[0]);
/// assert_eq!(6.0, q[1]);
/// assert_eq!(7.0, q[2]);
/// assert_eq!(8.0, q[3]);
/// ```
impl<T: Float> IndexMut<usize> for Quaternion<T> {
    fn index_mut(&mut self, index: usize) -> &mut Self::Output {
        return match index {
            0 => &mut self.w,
            1 => &mut self.x,
            2 => &mut self.y,
            3 => &mut self.z,
            _ => {
                panic!()
            }
        };
    }
}

/// Returns true if **other** is the same as self vector.
/// ```
/// use vox_geometry_rust::quaternion::QuaternionD;
/// let q = QuaternionD::new(1.0, 2.0, 3.0, 4.0);
/// let q2 = QuaternionD::new(1.0, 2.0, 3.0, 4.0);
/// assert_eq!(q == q2, true);
/// ```
impl<T: Float> PartialEq for Quaternion<T> {
    fn eq(&self, other: &Self) -> bool {
        return self.w == other.w &&
            self.x == other.x &&
            self.y == other.y &&
            self.z == other.z;
    }
}

/// ```
/// use vox_geometry_rust::quaternion::QuaternionD;
/// let q = QuaternionD::new(1.0, 2.0, 3.0, 4.0);
/// let q2 = QuaternionD::new(5.0, 6.0, 7.0, 8.0);
/// assert_eq!(q == q2, false);
/// ```
impl<T: Float> Eq for Quaternion<T> {}

/// # Builders
impl<T: Float> Quaternion<T> {
    /// Returns identity matrix.
    pub fn make_identity() -> Quaternion<T> {
        return Quaternion::new_default();
    }
}

/// Computes spherical linear interpolation.
pub fn slerp<T: Float>(a: Quaternion<T>,
                       b: Quaternion<T>,
                       t: T) -> Quaternion<T> {
    let threshold = T::from(0.01).unwrap();
    let eps = T::epsilon();

    let cos_half_angle = Quaternion::dot(&a, b);
    let weight_a: T;
    let weight_b: T;

    // For better accuracy, return lerp result when a and b are close enough.
    if T::one() - T::abs(cos_half_angle) < threshold {
        weight_a = T::one() - t;
        weight_b = t;
    } else {
        let half_angle = T::acos(cos_half_angle);
        let sin_half_angle = T::sqrt(T::one() - cos_half_angle * cos_half_angle);

        // In case of angle ~ 180, pick middle value.
        // If not, perform slerp.
        if T::abs(sin_half_angle) < eps {
            weight_a = T::from(0.5).unwrap();
            weight_b = T::from(0.5).unwrap();
        } else {
            weight_a = T::sin((T::one() - t) * half_angle) / sin_half_angle;
            weight_b = T::sin(t * half_angle) / sin_half_angle;
        }
    }

    return Quaternion::new(
        weight_a * a.w + weight_b * b.w,
        weight_a * a.x + weight_b * b.x,
        weight_a * a.y + weight_b * b.y,
        weight_a * a.z + weight_b * b.z);
}

/// Returns quaternion q * vector v.
impl<T: Float> Mul<Vector3<T>> for Quaternion<T> {
    type Output = Vector3<T>;
    fn mul(self, rhs: Vector3<T>) -> Self::Output {
        return self.mul_vec(rhs);
    }
}

/// Returns quaternion a times quaternion b.
impl<T: Float> Mul for Quaternion<T> {
    type Output = Quaternion<T>;
    fn mul(self, rhs: Self) -> Self::Output {
        return self.mul_quat(rhs);
    }
}

/// Returns this quaternion *= other quaternion.
/// ```
/// use vox_geometry_rust::quaternion::QuaternionD;
/// let mut q = QuaternionD::new(1.0, 2.0, 3.0, 4.0);
/// let q2 = QuaternionD::new(5.0, 6.0, 7.0, 8.0);
///
/// q *= q2;
///
/// assert_eq!(-60.0, q.w);
/// assert_eq!(12.0, q.x);
/// assert_eq!(30.0, q.y);
/// assert_eq!(24.0, q.z);
/// ```
impl<T: Float> MulAssign for Quaternion<T> {
    fn mul_assign(&mut self, rhs: Self) {
        return self.imul_quat(rhs);
    }
}

impl<T: Float + Debug> Debug for Quaternion<T> {
    /// # Example
    /// ```
    ///
    /// use vox_geometry_rust::quaternion::QuaternionD;
    /// let vec = QuaternionD::new(10.0, 20.0, 30.0, 40.0);
    /// assert_eq!(format!("{:?}", vec), "(10.0, 20.0, 30.0, 40.0)");
    ///
    /// assert_eq!(format!("{:#?}", vec), "(
    ///     10.0,
    ///     20.0,
    ///     30.0,
    ///     40.0,
    /// )");
    /// ```
    fn fmt(&self, f: &mut Formatter<'_>) -> Result {
        f.debug_tuple("")
            .field(&self.w)
            .field(&self.x)
            .field(&self.y)
            .field(&self.z)
            .finish()
    }
}