nalgebra/src/geometry/quaternion_construction.rs

#[cfg(feature = "arbitrary")]
use quickcheck::{Arbitrary, Gen};
#[cfg(feature = "arbitrary")]
use core::storage::Owned;
#[cfg(feature = "arbitrary")]
use core::dimension::U4;

use rand::{Rand, Rng};
use num::{Zero, One};

use alga::general::Real;

use core::{Unit, Vector, Vector4, Vector3};
use core::storage::Storage;
use core::dimension::U3;

use geometry::{Quaternion, UnitQuaternion, Rotation};

impl<N: Real> Quaternion<N> {
    /// Creates a quaternion from a 4D vector. The quaternion scalar part corresponds to the `w`
    /// vector component.
    #[inline]
    pub fn from_vector(vector: Vector4<N>) -> Self {
        Quaternion {
            coords: vector
        }
    }

    /// Creates a new quaternion from its individual components. Note that the arguments order does
    /// **not** follow the storage order.
    ///
    /// The storage order is `[ x, y, z, w ]`.
    #[inline]
    pub fn new(w: N, x: N, y: N, z: N) -> Self {
        let v = Vector4::<N>::new(x, y, z, w);
        Self::from_vector(v)
    }

    /// Creates a new quaternion from its scalar and vector parts. Note that the arguments order does
    /// **not** follow the storage order.
    ///
    /// The storage order is [ vector, scalar ].
    #[inline]
    // FIXME: take a reference to `vector`?
    pub fn from_parts<SB>(scalar: N, vector: Vector<N, U3, SB>) -> Self
        where SB: Storage<N, U3> {

        Self::new(scalar, vector[0], vector[1], vector[2])
    }

    /// Creates a new quaternion from its polar decomposition.
    ///
    /// Note that `axis` is assumed to be a unit vector.
    // FIXME: take a reference to `axis`?
    pub fn from_polar_decomposition<SB>(scale: N, theta: N, axis: Unit<Vector<N, U3, SB>>) -> Self
        where SB: Storage<N, U3> {
        let rot = UnitQuaternion::<N>::from_axis_angle(&axis, theta * ::convert(2.0f64));

        rot.unwrap() * scale
    }

    /// The quaternion multiplicative identity.
    #[inline]
    pub fn identity() -> Self {
        Self::new(N::one(), N::zero(), N::zero(), N::zero())
    }
}

impl<N: Real> One for Quaternion<N> {
    #[inline]
    fn one() -> Self {
        Self::identity()
    }
}

impl<N: Real> Zero for Quaternion<N> {
    #[inline]
    fn zero() -> Self {
        Self::new(N::zero(), N::zero(), N::zero(), N::zero())
    }

    #[inline]
    fn is_zero(&self) -> bool {
        self.coords.is_zero()
    }
}

impl<N: Real + Rand> Rand for Quaternion<N> {
    #[inline]
    fn rand<R: Rng>(rng: &mut R) -> Self {
        Quaternion::new(rng.gen(), rng.gen(), rng.gen(), rng.gen())
    }
}

#[cfg(feature="arbitrary")]
impl<N: Real + Arbitrary> Arbitrary for Quaternion<N>
    where Owned<N, U4>: Send {
    #[inline]
    fn arbitrary<G: Gen>(g: &mut G) -> Self {
        Quaternion::new(N::arbitrary(g), N::arbitrary(g),
                            N::arbitrary(g), N::arbitrary(g))
    }
}

impl<N: Real> UnitQuaternion<N> {
    /// The quaternion multiplicative identity.
    #[inline]
    pub fn identity() -> Self {
        Self::new_unchecked(Quaternion::identity())
    }

    /// Creates a new quaternion from a unit vector (the rotation axis) and an angle
    /// (the rotation angle).
    #[inline]
    pub fn from_axis_angle<SB>(axis: &Unit<Vector<N, U3, SB>>, angle: N) -> Self
        where SB: Storage<N, U3> {
        let (sang, cang) = (angle / ::convert(2.0f64)).sin_cos();

        let q = Quaternion::from_parts(cang, axis.as_ref() * sang);
        Self::new_unchecked(q)
    }

    /// Creates a new unit quaternion from a quaternion.
    ///
    /// The input quaternion will be normalized.
    #[inline]
    pub fn from_quaternion(q: Quaternion<N>) -> Self {
        Self::new_normalize(q)
    }

    /// Creates a new unit quaternion from Euler angles.
    ///
    /// The primitive rotations are applied in order: 1 roll − 2 pitch − 3 yaw.
    #[inline]
    pub fn from_euler_angles(roll: N, pitch: N, yaw: N) -> Self {
        let (sr, cr) = (roll  * ::convert(0.5f64)).sin_cos();
        let (sp, cp) = (pitch * ::convert(0.5f64)).sin_cos();
        let (sy, cy) = (yaw   * ::convert(0.5f64)).sin_cos();

        let q = Quaternion::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);

        Self::new_unchecked(q)
    }

    /// Builds an unit quaternion from a rotation matrix.
    #[inline]
    pub fn from_rotation_matrix(rotmat: &Rotation<N, U3>) -> Self {
        // Robust matrix to quaternion transformation.
        // See http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion
        let tr = rotmat[(0, 0)] + rotmat[(1, 1)] + rotmat[(2, 2)];
        let res;

        let _0_25: N = ::convert(0.25);

        if tr > N::zero() {
          let denom = (tr + N::one()).sqrt() * ::convert(2.0);
          res = Quaternion::new(_0_25 * denom,
                                    (rotmat[(2, 1)] - rotmat[(1, 2)]) / denom,
                                    (rotmat[(0, 2)] - rotmat[(2, 0)]) / denom,
                                    (rotmat[(1, 0)] - rotmat[(0, 1)]) / denom);
        }
        else if rotmat[(0, 0)] > rotmat[(1, 1)] && rotmat[(0, 0)] > rotmat[(2, 2)] {
          let denom = (N::one() + rotmat[(0, 0)] - rotmat[(1, 1)] - rotmat[(2, 2)]).sqrt() * ::convert(2.0);
          res = Quaternion::new((rotmat[(2, 1)] - rotmat[(1, 2)]) / denom,
                                    _0_25 * denom,
                                    (rotmat[(0, 1)] + rotmat[(1, 0)]) / denom,
                                    (rotmat[(0, 2)] + rotmat[(2, 0)]) / denom);
        }
        else if rotmat[(1, 1)] > rotmat[(2, 2)] {
          let denom = (N::one() + rotmat[(1, 1)] - rotmat[(0, 0)] - rotmat[(2, 2)]).sqrt() * ::convert(2.0);
          res = Quaternion::new((rotmat[(0, 2)] - rotmat[(2, 0)]) / denom,
                                    (rotmat[(0, 1)] + rotmat[(1, 0)]) / denom,
                                    _0_25 * denom,
                                    (rotmat[(1, 2)] + rotmat[(2, 1)]) / denom);
        }
        else {
          let denom = (N::one() + rotmat[(2, 2)] - rotmat[(0, 0)] - rotmat[(1, 1)]).sqrt() * ::convert(2.0);
          res = Quaternion::new((rotmat[(1, 0)] - rotmat[(0, 1)]) / denom,
                                    (rotmat[(0, 2)] + rotmat[(2, 0)]) / denom,
                                    (rotmat[(1, 2)] + rotmat[(2, 1)]) / denom,
                                    _0_25 * denom);
        }

        Self::new_unchecked(res)
    }

    /// The unit quaternion needed to make `a` and `b` be collinear and point toward the same
    /// direction.
    #[inline]
    pub fn rotation_between<SB, SC>(a: &Vector<N, U3, SB>, b: &Vector<N, U3, SC>) -> Option<Self>
        where SB: Storage<N, U3>,
              SC: Storage<N, U3> {
        Self::scaled_rotation_between(a, b, N::one())
    }

    /// The smallest rotation needed to make `a` and `b` collinear and point toward the same
    /// direction, raised to the power `s`.
    #[inline]
    pub fn scaled_rotation_between<SB, SC>(a: &Vector<N, U3, SB>,
                                           b: &Vector<N, U3, SC>,
                                           s: N)
                                           -> Option<Self>
        where SB: Storage<N, U3>,
              SC: Storage<N, U3> {
        // FIXME: code duplication with Rotation.
        if let (Some(na), Some(nb)) = (Unit::try_new(a.clone_owned(), N::zero()),
                                       Unit::try_new(b.clone_owned(), N::zero())) {
            Self::scaled_rotation_between_axis(&na, &nb, s)
        }
        else {
            Some(Self::identity())
        }
    }

    /// The unit quaternion needed to make `a` and `b` be collinear and point toward the same
    /// direction.
    #[inline]
    pub fn rotation_between_axis<SB, SC>(a: &Unit<Vector<N, U3, SB>>, b: &Unit<Vector<N, U3, SC>>) -> Option<Self>
        where SB: Storage<N, U3>,
              SC: Storage<N, U3> {
        Self::scaled_rotation_between_axis(a, b, N::one())
    }

    /// The smallest rotation needed to make `a` and `b` collinear and point toward the same
    /// direction, raised to the power `s`.
    #[inline]
    pub fn scaled_rotation_between_axis<SB, SC>(na: &Unit<Vector<N, U3, SB>>,
                                                nb: &Unit<Vector<N, U3, SC>>,
                                                s:  N)
                                                -> Option<Self>
        where SB: Storage<N, U3>,
              SC: Storage<N, U3> {

        // FIXME: code duplication with Rotation.
        let c = na.cross(&nb);

        if let Some(axis) = Unit::try_new(c, N::default_epsilon()) {
            let cos = na.dot(&nb);

            // The cosinus may be out of [-1, 1] because of innacuracies.
            if cos <= -N::one() {
                return None
            }
            else if cos >= N::one() {
                return Some(Self::identity())
            }
            else {
                return Some(Self::from_axis_angle(&axis, cos.acos() * s))
            }
        }
        else if na.dot(&nb) < N::zero() {
            // PI
            //
            // The rotation axis is undefined but the angle not zero. This is not a
            // simple rotation.
            return None;
        }
        else {
            // Zero
            Some(Self::identity())
        }
    }


    /// Creates an unit quaternion that corresponds to the local frame of an observer standing at the
    /// origin and looking toward `dir`.
    ///
    /// It maps the view direction `dir` to the positive `z` axis.
    ///
    /// # Arguments
    ///   * dir - The look direction, that is, direction the matrix `z` axis will be aligned with.
    ///   * up - The vertical direction. The only requirement of this parameter is to not be
    ///   collinear
    ///   to `dir`. Non-collinearity is not checked.
    #[inline]
    pub fn new_observer_frame<SB, SC>(dir: &Vector<N, U3, SB>, up: &Vector<N, U3, SC>) -> Self
    where SB: Storage<N, U3>,
          SC: Storage<N, U3> {
        Self::from_rotation_matrix(&Rotation::<N, U3>::new_observer_frame(dir, up))
    }


    /// Builds a right-handed look-at view matrix without translation.
    ///
    /// This conforms to the common notion of right handed look-at matrix from the computer
    /// graphics community.
    ///
    /// # Arguments
    ///   * eye - The eye position.
    ///   * target - The target position.
    ///   * up - A vector approximately aligned with required the vertical axis. The only
    ///   requirement of this parameter is to not be collinear to `target - eye`.
    #[inline]
    pub fn look_at_rh<SB, SC>(dir: &Vector<N, U3, SB>, up: &Vector<N, U3, SC>) -> Self
    where SB: Storage<N, U3>,
          SC: Storage<N, U3> {
        Self::new_observer_frame(&-dir, up).inverse()
    }

    /// Builds a left-handed look-at view matrix without translation.
    ///
    /// This conforms to the common notion of left handed look-at matrix from the computer
    /// graphics community.
    ///
    /// # Arguments
    ///   * eye - The eye position.
    ///   * target - The target position.
    ///   * up - A vector approximately aligned with required the vertical axis. The only
    ///   requirement of this parameter is to not be collinear to `target - eye`.
    #[inline]
    pub fn look_at_lh<SB, SC>(dir: &Vector<N, U3, SB>, up: &Vector<N, U3, SC>) -> Self
    where SB: Storage<N, U3>,
          SC: Storage<N, U3> {
            Self::new_observer_frame(dir, up).inverse()
    }

    /// Creates a new unit quaternion rotation from a rotation axis scaled by the rotation angle.
    ///
    /// If `axisangle` is zero, this returns the indentity rotation.
    #[inline]
    pub fn new<SB>(axisangle: Vector<N, U3, SB>) -> Self
        where SB: Storage<N, U3> {
        let two: N = ::convert(2.0f64);
        let q = Quaternion::<N>::from_parts(N::zero(), axisangle / two).exp();
        Self::new_unchecked(q)
    }

    /// Creates a new unit quaternion rotation from a rotation axis scaled by the rotation angle.
    ///
    /// If `axisangle` is zero, this returns the indentity rotation.
    /// Same as `Self::new(axisangle)`.
    #[inline]
    pub fn from_scaled_axis<SB>(axisangle: Vector<N, U3, SB>) -> Self
        where SB: Storage<N, U3> {
        Self::new(axisangle)
    }
}

impl<N: Real> One for UnitQuaternion<N> {
    #[inline]
    fn one() -> Self {
        Self::identity()
    }
}

impl<N: Real + Rand> Rand for UnitQuaternion<N> {
    #[inline]
    fn rand<R: Rng>(rng: &mut R) -> Self {
        let axisangle = Vector3::rand(rng);
        UnitQuaternion::from_scaled_axis(axisangle)
    }
}

#[cfg(feature="arbitrary")]
impl<N: Real + Arbitrary> Arbitrary for UnitQuaternion<N>
    where Owned<N, U4>: Send,
          Owned<N, U3>: Send {
    #[inline]
    fn arbitrary<G: Gen>(g: &mut G) -> Self {
        let axisangle = Vector3::arbitrary(g);
        UnitQuaternion::from_scaled_axis(axisangle)

    }
}