Quaternion.hpp

Go to the documentation of this file.

/**************************************************************************************/
/*                                                                                    */
/*  Visualization Library                                                             */
/*  http://visualizationlibrary.org                                                   */
/*                                                                                    */
/*  Copyright (c) 2005-2020, Michele Bosi                                             */
/*  All rights reserved.                                                              */
/*                                                                                    */
/*  Redistribution and use in source and binary forms, with or without modification,  */
/*  are permitted provided that the following conditions are met:                     */
/*                                                                                    */
/*  - Redistributions of source code must retain the above copyright notice, this     */
/*  list of conditions and the following disclaimer.                                  */
/*                                                                                    */
/*  - Redistributions in binary form must reproduce the above copyright notice, this  */
/*  list of conditions and the following disclaimer in the documentation and/or       */
/*  other materials provided with the distribution.                                   */
/*                                                                                    */
/*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND   */
/*  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED     */
/*  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE            */
/*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR  */
/*  ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES    */
/*  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;      */
/*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON    */
/*  ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT           */
/*  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS     */
/*  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                      */
/*                                                                                    */
/**************************************************************************************/

#ifndef Quaternion_INCLUDE_ONCE
#define Quaternion_INCLUDE_ONCE

#include <vlCore/glsl_math.hpp>

namespace vl
{
  //-----------------------------------------------------------------------------
  // Quaternion
  //-----------------------------------------------------------------------------
  template<typename T_Scalar>
  class Quaternion
  {
  public:
    typedef T_Scalar scalar_type;
    //-----------------------------------------------------------------------------
    Quaternion()
    {
      setNoRotation();
    }
    //-----------------------------------------------------------------------------
    template<typename T>
    explicit Quaternion(const Quaternion<T>& quat)
    {
      mXYZW.x() = (T_Scalar)quat.xyzw().x();
      mXYZW.y() = (T_Scalar)quat.xyzw().y();
      mXYZW.z() = (T_Scalar)quat.xyzw().z();
      mXYZW.w() = (T_Scalar)quat.xyzw().w();
    }
    //-----------------------------------------------------------------------------
    explicit Quaternion(T_Scalar x, T_Scalar y, T_Scalar z, T_Scalar w)
    {
      mXYZW.x() = x;
      mXYZW.y() = y;
      mXYZW.z() = z;
      mXYZW.w() = w;
    }
    //-----------------------------------------------------------------------------
    explicit Quaternion(T_Scalar degrees, const Vector3<T_Scalar>& axis)
    {
      setFromAxisAngle(axis, degrees);
    }
    //-----------------------------------------------------------------------------
    explicit Quaternion(const Vector4<T_Scalar>& v)
    {
      mXYZW.x() = (T_Scalar)v.x();
      mXYZW.y() = (T_Scalar)v.y();
      mXYZW.z() = (T_Scalar)v.z();
      mXYZW.w() = (T_Scalar)v.w();
    }
    //-----------------------------------------------------------------------------
    Quaternion& operator=(const Quaternion& q)
    {
      mXYZW.x() = q.x();
      mXYZW.y() = q.y();
      mXYZW.z() = q.z();
      mXYZW.w() = q.w();
      return *this;
    }
    //-----------------------------------------------------------------------------
    Quaternion& operator=(const Vector4<T_Scalar>& v)
    {
      mXYZW.x() = (T_Scalar)v.x();
      mXYZW.y() = (T_Scalar)v.y();
      mXYZW.z() = (T_Scalar)v.z();
      mXYZW.w() = (T_Scalar)v.w();
      return *this;
    }
    //-----------------------------------------------------------------------------
    bool operator==(const Quaternion& q) const
    {
      return x() == q.x() && y() == q.y() && z() == q.z() && w() == q.w();
    }
    //-----------------------------------------------------------------------------
    bool operator!=(const Quaternion& q) const
    {
      return !operator==(q);
    }
    //-----------------------------------------------------------------------------
    bool operator<(const Quaternion& other) const
    {
      if (x() != other.x())
        return x() < other.x();
      if (y() != other.y())
        return y() < other.y();
      if (z() != other.z())
        return z() < other.z();
      else
        return w() < other.w();
    }
    //-----------------------------------------------------------------------------
    const Vector4<T_Scalar>& xyzw() const { return mXYZW; }
    //-----------------------------------------------------------------------------
    Vector4<T_Scalar>& xyzw() { return mXYZW; }
    //-----------------------------------------------------------------------------
    T_Scalar& x() { return mXYZW.x(); }
    //-----------------------------------------------------------------------------
    T_Scalar& y() { return mXYZW.y(); }
    //-----------------------------------------------------------------------------
    T_Scalar& z() { return mXYZW.z(); }
    //-----------------------------------------------------------------------------
    T_Scalar& w() { return mXYZW.w(); }
    //-----------------------------------------------------------------------------
    const T_Scalar& x() const { return mXYZW.x(); }
    //-----------------------------------------------------------------------------
    const T_Scalar& y() const { return mXYZW.y(); }
    //-----------------------------------------------------------------------------
    const T_Scalar& z() const { return mXYZW.z(); }
    //-----------------------------------------------------------------------------
    const T_Scalar& w() const { return mXYZW.w(); }
    //-----------------------------------------------------------------------------
    Quaternion operator*(T_Scalar val) const
    {
      Quaternion t = *this;
      t.x() *= val;
      t.y() *= val;
      t.z() *= val;
      t.w() *= val;
      return t;
    }
    //-----------------------------------------------------------------------------
    Quaternion& operator*=(T_Scalar val)
    {
      x() *= val;
      y() *= val;
      z() *= val;
      w() *= val;
      return *this;
    }
    //-----------------------------------------------------------------------------
    Quaternion operator/(T_Scalar val) const
    {
      Quaternion t = *this;
      val = (T_Scalar)1.0 / val;
      t.x() *= val;
      t.y() *= val;
      t.z() *= val;
      t.w() *= val;
      return t;
    }
    //-----------------------------------------------------------------------------
    Quaternion& operator/=(T_Scalar val)
    {
      val = (T_Scalar)1.0 / val;
      x() *= val;
      y() *= val;
      z() *= val;
      w() *= val;
      return *this;
    }
    //-----------------------------------------------------------------------------
    Quaternion operator+(const Quaternion& q) const
    {
      Quaternion t = *this;
      t.x() += q.x();
      t.y() += q.y();
      t.z() += q.z();
      t.w() += q.w();
      return t;
    }
    //-----------------------------------------------------------------------------
    Quaternion& operator+=(const Quaternion& q)
    {
      x() += q.x();
      y() += q.y();
      z() += q.z();
      w() += q.w();
      return *this;
    }
    //-----------------------------------------------------------------------------
    Quaternion operator-(const Quaternion& q) const
    {
      Quaternion t = *this;
      t.x() -= q.x();
      t.y() -= q.y();
      t.z() -= q.z();
      t.w() -= q.w();
      return t;
    }
    //-----------------------------------------------------------------------------
    Quaternion& operator-=(const Quaternion& q)
    {
      x() -= q.x();
      y() -= q.y();
      z() -= q.z();
      w() -= q.w();
      return *this;
    }
    //-----------------------------------------------------------------------------
    Quaternion operator-() const
    {
      return Quaternion(-x(), -y(), -z(), -w());
    }
    //-----------------------------------------------------------------------------
    Quaternion& setZero()
    {
      mXYZW.x() = 0;
      mXYZW.y() = 0;
      mXYZW.z() = 0;
      mXYZW.w() = 0;
      return *this;
    }
    //-----------------------------------------------------------------------------
    static Quaternion getZero()
    {
      return Quaternion().setZero();
    }
    //-----------------------------------------------------------------------------
    static Quaternion& getZero(Quaternion& q)
    {
      return q.setZero();
    }
    //-----------------------------------------------------------------------------
    Quaternion& setNoRotation()
    {
      mXYZW.x() = 0;
      mXYZW.y() = 0;
      mXYZW.z() = 0;
      mXYZW.w() = 1;
      return *this;
    }
    //-----------------------------------------------------------------------------
    static Quaternion getNoRotation()
    {
      return Quaternion();
    }
    //-----------------------------------------------------------------------------
    static Quaternion& getNoRotation(Quaternion& q)
    {
      return q.setNoRotation();
    }
    //-----------------------------------------------------------------------------
    Quaternion& setFromVectors(const Vector3<T_Scalar>& from, const Vector3<T_Scalar>& to);
    //-----------------------------------------------------------------------------
    static Quaternion getFromVectors(const Vector3<T_Scalar>& from, const Vector3<T_Scalar>& to)
    {
      return Quaternion().setFromVectors(from, to);
    }
    //-----------------------------------------------------------------------------
    static Quaternion& getFromVectors(Quaternion& q, const Vector3<T_Scalar>& from, const Vector3<T_Scalar>& to)
    {
      return q.setFromVectors(from, to);
    }
    //-----------------------------------------------------------------------------
    Quaternion& setFromMatrix(const Matrix4<T_Scalar>& m);
    //-----------------------------------------------------------------------------
    static Quaternion getFromMatrix(const Matrix4<T_Scalar>& m)
    {
      return Quaternion().setFromMatrix(m);
    }
    //-----------------------------------------------------------------------------
    static Quaternion& getFromMatrix(Quaternion& q, const Matrix4<T_Scalar>& m)
    {
      return q.setFromMatrix(m);
    }
    //-----------------------------------------------------------------------------
    Quaternion& setFromMatrix(const Matrix3<T_Scalar>& m);
    //-----------------------------------------------------------------------------
    static Quaternion getFromMatrix(const Matrix3<T_Scalar>& m)
    {
      return Quaternion().setFromMatrix(m);
    }
    //-----------------------------------------------------------------------------
    static Quaternion& getFromMatrix(Quaternion& q, const Matrix3<T_Scalar>& m)
    {
      return q.setFromMatrix(m);
    }
    //-----------------------------------------------------------------------------
    Quaternion& setFromEulerXYZ(T_Scalar degX, T_Scalar degY, T_Scalar degZ);
    //-----------------------------------------------------------------------------
    static Quaternion getFromEulerXYZ(T_Scalar degX, T_Scalar degY, T_Scalar degZ)
    {
      return Quaternion().setFromEulerXYZ(degX, degY, degZ);
    }
    //-----------------------------------------------------------------------------
    static Quaternion& getFromEulerXYZ(Quaternion& q, T_Scalar degX, T_Scalar degY, T_Scalar degZ)
    {
      return q.setFromEulerXYZ(degX, degY, degZ);
    }
    //-----------------------------------------------------------------------------
    Quaternion& setFromEulerZYX(T_Scalar degZ, T_Scalar degY, T_Scalar degX);
    //-----------------------------------------------------------------------------
    static Quaternion getFromEulerZYX(T_Scalar degZ, T_Scalar degY, T_Scalar degX)
    {
      return Quaternion().setFromEulerZYX(degZ, degY, degX);
    }
    //-----------------------------------------------------------------------------
    static Quaternion& getFromEulerZYX(Quaternion& q, T_Scalar degZ, T_Scalar degY, T_Scalar degX)
    {
      return q.setFromEulerZYX(degZ, degY, degX);
    }
    //-----------------------------------------------------------------------------
    Quaternion& setFromAxisAngle(const Vector3<T_Scalar>& axis, T_Scalar degrees);
    //-----------------------------------------------------------------------------
    static Quaternion getFromAxisAngle(const Vector3<T_Scalar>& axis, T_Scalar degrees)
    {
      return Quaternion().setFromAxisAngle(axis, degrees);
    }
    //-----------------------------------------------------------------------------
    static Quaternion& getFromAxisAngle(Quaternion& q, const Vector3<T_Scalar>& axis, T_Scalar degrees)
    {
      return q.setFromAxisAngle(axis, degrees);
    }
    //-----------------------------------------------------------------------------
    void toAxisAngle( Vector3<T_Scalar>& axis, T_Scalar& degrees ) const;
    //-----------------------------------------------------------------------------
    Matrix4<T_Scalar> toMatrix4() const;
    //-----------------------------------------------------------------------------
    Matrix4<T_Scalar>& toMatrix4(Matrix4<T_Scalar>&) const;
    //-----------------------------------------------------------------------------
    Matrix3<T_Scalar> toMatrix3() const;
    //-----------------------------------------------------------------------------
    Matrix3<T_Scalar>& toMatrix3(Matrix3<T_Scalar>&) const;
    //-----------------------------------------------------------------------------
    T_Scalar dot(const Quaternion& q) const
    {
      return x()*q.x() + y()*q.y() + z()*q.z() + w()*q.w();
    }
    //-----------------------------------------------------------------------------
    T_Scalar length() const  { return mXYZW.length(); }
    //-----------------------------------------------------------------------------
    Quaternion& normalize(T_Scalar* len=NULL) { mXYZW.normalize(len); return *this; }
    //-----------------------------------------------------------------------------
    Quaternion getNormalized(T_Scalar* len=NULL) const { Quaternion t = *this; t.normalize(len); return t; }
    //-----------------------------------------------------------------------------
    Quaternion& getNormalized(Quaternion& q, T_Scalar* len=NULL) const { q = *this; q.normalize(len); return q; }
    //-----------------------------------------------------------------------------
    T_Scalar lengthSquared() const
    {
      return x()*x() + y()*y() + z()*z() + w()*w();
    }
    //-----------------------------------------------------------------------------
    Quaternion getConjugate() const
    {
      return Quaternion(-x(), -y(), -z(), w());
    }
    //-----------------------------------------------------------------------------
    Quaternion& getConjugate(Quaternion& q) const
    {
      q = Quaternion(-x(), -y(), -z(), w());
      return q;
    }
    //-----------------------------------------------------------------------------
    Quaternion getInverse() const
    {
      return getConjugate() / lengthSquared();
    }
    //-----------------------------------------------------------------------------
    Quaternion& getInverse(Quaternion& q) const
    {
      q = getConjugate() / lengthSquared();
      return q;
    }
    //-----------------------------------------------------------------------------
    static Quaternion getSlerp(T_Scalar t, const Quaternion& a, const Quaternion& b);
    //-----------------------------------------------------------------------------
    static Quaternion& getSlerp(Quaternion& out, T_Scalar t, const Quaternion& a, const Quaternion& b);
    //-----------------------------------------------------------------------------
    static Quaternion getSquad(T_Scalar t, const Quaternion& a, const Quaternion& p, const Quaternion& q, const Quaternion& b)
    {
      return getSlerp((T_Scalar)2.0*t*((T_Scalar)1.0-t), getSlerp(t,a,b), getSlerp(t,p,q));
    }
    //-----------------------------------------------------------------------------
    static Quaternion& getSquad(Quaternion& out, T_Scalar t, const Quaternion& a, const Quaternion& p, const Quaternion& q, const Quaternion& b)
    {
      return getSlerp(out, (T_Scalar)2.0*t*((T_Scalar)1.0-t), getSlerp(t,a,b), getSlerp(t,p,q));
    }
    //-----------------------------------------------------------------------------
    static Quaternion getNlerp( T_Scalar t, const Quaternion& a, const Quaternion& b )
    {
      Quaternion q = a + (b - a) * t;
      q.normalize();
      return q;
    }
    //-----------------------------------------------------------------------------
    static Quaternion& getNlerp( Quaternion& out, T_Scalar t, const Quaternion& a, const Quaternion& b )
    {
      out = a + (b - a) * t;
      out.normalize();
      return out;
    }
    //-----------------------------------------------------------------------------

  protected:
    Vector4<T_Scalar> mXYZW;
  };
  //-----------------------------------------------------------------------------
  template<typename T_Scalar>
  inline Quaternion<T_Scalar> operator*(T_Scalar r, const Quaternion<T_Scalar>& q)
  {
    return q * r;
  }
  //-----------------------------------------------------------------------------
  template<typename T_Scalar>
  inline Quaternion<T_Scalar> operator*(const Quaternion<T_Scalar>& q1, const Quaternion<T_Scalar>& q2)
  {
    Quaternion<T_Scalar> q;
    q.x() = q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y();
    q.y() = q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z();
    q.z() = q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x();
    q.w() = q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z();
    return q;
  }
  //-----------------------------------------------------------------------------
  // post multiplication
  template<typename T_Scalar>
  inline Vector3<T_Scalar> operator*(const Quaternion<T_Scalar>&q, const Vector3<T_Scalar>& v)
  {
    // Matrix conversion formula based implementation
    T_Scalar x2 = q.x() * q.x();
    T_Scalar y2 = q.y() * q.y();
    T_Scalar z2 = q.z() * q.z();
    T_Scalar xy = q.x() * q.y();
    T_Scalar xz = q.x() * q.z();
    T_Scalar yz = q.y() * q.z();
    T_Scalar wx = q.w() * q.x();
    T_Scalar wy = q.w() * q.y();
    T_Scalar wz = q.w() * q.z();

    Vector3<T_Scalar> r;
    r.x() = ( v.x()*(1.0f - 2.0f * (y2 + z2)) + v.y()*(2.0f * (xy - wz)) + v.z()*(2.0f * (xz + wy)) );
    r.y() = ( v.x()*(2.0f * (xy + wz)) + v.y()*(1.0f - 2.0f * (x2 + z2)) + v.z()*(2.0f * (yz - wx)) );
    r.z() = ( v.x()*(2.0f * (xz - wy)) + v.y()*(2.0f * (yz + wx)) + v.z()*(1.0f - 2.0f * (x2 + y2)) );
    return r;
  }
  //-----------------------------------------------------------------------------
  // post multiplication
  template<typename T_Scalar>
  inline Vector4<T_Scalar> operator*(const Quaternion<T_Scalar>&q, const Vector4<T_Scalar>& v)
  {
    return Vector4<T_Scalar>( q * v.xyz(), v.w() );
  }
  //-----------------------------------------------------------------------------
  template<typename T_Scalar>
  Quaternion<T_Scalar>& Quaternion<T_Scalar>::setFromEulerXYZ(T_Scalar degX, T_Scalar degY, T_Scalar degZ )
  {
    *this = Quaternion<T_Scalar>(degX, Vector3<T_Scalar>(1,0,0)) * Quaternion<T_Scalar>(degY, Vector3<T_Scalar>(0,1,0)) * Quaternion<T_Scalar>(degZ, Vector3<T_Scalar>(0,0,1));
    return *this;
  }
  //-----------------------------------------------------------------------------
  template<typename T_Scalar>
  Quaternion<T_Scalar>& Quaternion<T_Scalar>::setFromEulerZYX(T_Scalar degZ, T_Scalar degY, T_Scalar degX )
  {
    *this = Quaternion<T_Scalar>(degZ, Vector3<T_Scalar>(0,0,1)) * Quaternion<T_Scalar>(degY, Vector3<T_Scalar>(0,1,0)) * Quaternion<T_Scalar>(degX, Vector3<T_Scalar>(1,0,0));
    return *this;
  }
  //-----------------------------------------------------------------------------
  template<typename T_Scalar>
  Quaternion<T_Scalar>& Quaternion<T_Scalar>::setFromMatrix(const Matrix4<T_Scalar>& m)
  {
    return setFromMatrix( m.get3x3() );
  }
  //-----------------------------------------------------------------------------
  template<typename T_Scalar>
  Quaternion<T_Scalar>& Quaternion<T_Scalar>::setFromMatrix(const Matrix3<T_Scalar>& m)
  {
    T_Scalar tr, s, q[4];

    int next[3] = {1, 2, 0};

    tr = m.e(0,0) + m.e(1,1) + m.e(2,2);

    // check the diagonal
    if (tr + (T_Scalar)1.0 > 0.0)
    {
      s = vl::sqrt(tr + (T_Scalar)1.0);
      w() = s / (T_Scalar)2.0;
      s = (T_Scalar)0.5 / s;
      x() = (m.e(2,1) - m.e(1,2)) * s;
      y() = (m.e(0,2) - m.e(2,0)) * s;
      z() = (m.e(1,0) - m.e(0,1)) * s;
    }
    else
    {
      // diagonal is negative
      int i, j, k;
      i = 0;
      if (m.e(1,1) > m.e(0,0)) i = 1;
      if (m.e(2,2) > m.e(i,i)) i = 2;
      j = next[i];
      k = next[j];

      s = vl::sqrt((m.e(i,i) - (m.e(j,j) + m.e(k,k))) + (T_Scalar)1.0);

      q[i] = s * (T_Scalar)0.5;

      if (s != 0.0)
        s = (T_Scalar)0.5 / s;

      q[3] = (m.e(k,j) - m.e(j,k)) * s;
      q[j] = (m.e(j,i) + m.e(i,j)) * s;
      q[k] = (m.e(k,i) + m.e(i,k)) * s;

      x() = q[0];
      y() = q[1];
      z() = q[2];
      w() = q[3];
    }

    return *this;
  }
  //-----------------------------------------------------------------------------
  template<typename T_Scalar>
  Quaternion<T_Scalar>& Quaternion<T_Scalar>::setFromAxisAngle( const Vector3<T_Scalar>& axis, T_Scalar degrees )
  {
    degrees *= (T_Scalar)dDEG_TO_RAD;
    T_Scalar sa2 = sin(degrees * (T_Scalar)0.5);
    Vector3<T_Scalar> na = axis;
    na.normalize();
    mXYZW.x() = na.x() * sa2;
    mXYZW.y() = na.y() * sa2;
    mXYZW.z() = na.z() * sa2;
    mXYZW.w() = cos(degrees * (T_Scalar)0.5);
    return *this;
  }
  //-----------------------------------------------------------------------------
  template<typename T_Scalar>
  void Quaternion<T_Scalar>::toAxisAngle( Vector3<T_Scalar>& axis, T_Scalar& degrees ) const
  {
    T_Scalar iscale = sqrt( x()*x() + y()*y() + z()*z() );
    if (iscale == 0)
    {
      axis.x() = 0;
      axis.y() = 0;
      axis.z() = 0;
      degrees  = 0;
    }
    else
    {
      iscale = T_Scalar(1.0) / iscale;
      axis.x() = x() * iscale;
      axis.y() = y() * iscale;
      axis.z() = z() * iscale;
      VL_CHECK(w()>=-1.0 && w()<=+1.0)
      T_Scalar tw = clamp(w(),(T_Scalar)-1.0,(T_Scalar)+1.0);
      degrees = acos( tw ) * (T_Scalar)2.0 * (T_Scalar)dRAD_TO_DEG;
    }
  }
  //-----------------------------------------------------------------------------
  template<typename T_Scalar>
  Matrix4<T_Scalar>& Quaternion<T_Scalar>::toMatrix4(Matrix4<T_Scalar>& out) const
  {
    T_Scalar x2 = x() * x();
    T_Scalar y2 = y() * y();
    T_Scalar z2 = z() * z();
    T_Scalar xy = x() * y();
    T_Scalar xz = x() * z();
    T_Scalar yz = y() * z();
    T_Scalar wx = w() * x();
    T_Scalar wy = w() * y();
    T_Scalar wz = w() * z();

    return out = Matrix4<T_Scalar>(
      (1.0f - 2.0f * (y2 + z2)), (2.0f * (xy - wz)),        (2.0f * (xz + wy)),        0.0f,
      (2.0f * (xy + wz)),        (1.0f - 2.0f * (x2 + z2)), (2.0f * (yz - wx)),        0.0f,
      (2.0f * (xz - wy)),        (2.0f * (yz + wx)),        (1.0f - 2.0f * (x2 + y2)), 0.0f,
      0.0f,                      0.0f,                      0.0f,                      1.0f );
  }
  //-----------------------------------------------------------------------------
  template<typename T_Scalar>
  Matrix4<T_Scalar> Quaternion<T_Scalar>::toMatrix4() const
  {
    Matrix4<T_Scalar> out;
    return toMatrix4(out);
  }
  //-----------------------------------------------------------------------------
  template<typename T_Scalar>
  Matrix3<T_Scalar>& Quaternion<T_Scalar>::toMatrix3(Matrix3<T_Scalar>& out) const
  {
    T_Scalar x2 = x() * x();
    T_Scalar y2 = y() * y();
    T_Scalar z2 = z() * z();
    T_Scalar xy = x() * y();
    T_Scalar xz = x() * z();
    T_Scalar yz = y() * z();
    T_Scalar wx = w() * x();
    T_Scalar wy = w() * y();
    T_Scalar wz = w() * z();

    return out = Matrix3<T_Scalar>(
      (1.0f - 2.0f * (y2 + z2)), (2.0f * (xy + wz)),        (2.0f * (xz - wy)),
      (2.0f * (xy - wz)),        (1.0f - 2.0f * (x2 + z2)), (2.0f * (yz + wx)),
      (2.0f * (xz + wy)),        (2.0f * (yz - wx)),        (1.0f - 2.0f * (x2 + y2)) );
  }
  //-----------------------------------------------------------------------------
  template<typename T_Scalar>
  Matrix3<T_Scalar> Quaternion<T_Scalar>::toMatrix3() const
  {
    Matrix3<T_Scalar> out;
    return toMatrix3(out);
  }
  //-----------------------------------------------------------------------------
  template<typename T_Scalar>
  Quaternion<T_Scalar> Quaternion<T_Scalar>::getSlerp( T_Scalar t, const Quaternion<T_Scalar>& a, const Quaternion<T_Scalar>& b )
  {
    Quaternion<T_Scalar> q;
    getSlerp(q, t, a, b);
    return q;
  }
  //-----------------------------------------------------------------------------
  template<typename T_Scalar>
  Quaternion<T_Scalar>& Quaternion<T_Scalar>::getSlerp( Quaternion<T_Scalar>& out, T_Scalar t, const Quaternion<T_Scalar>& a, const Quaternion<T_Scalar>& b )
  {
    T_Scalar scale_a, scale_b, omega, sinom;
    T_Scalar cosom = a.dot(b);

    Quaternion<T_Scalar> b2(b);

    if ( cosom < 0 )
    {
      cosom = -cosom;
      b2 = -b;
    }

    // clamp rounding errors
    cosom = cosom > (T_Scalar)1 ? (T_Scalar)1 : cosom;

    if( cosom < (T_Scalar)1.0 )
    {
      omega = acos(cosom);
      sinom = sin(omega);
      VL_CHECK(sinom != 0)
        scale_a = sin(((T_Scalar)1.0-t) * omega) / sinom;
      scale_b = sin(t * omega) / sinom;
    }
    else
    {
      // linear interpolation for degenerate cases
      scale_a = (T_Scalar)1.0 - t;
      scale_b = t;
    }

    return out = (a*scale_a) + (b2*scale_b);
  }
  //-----------------------------------------------------------------------------
  template<typename T_Scalar>
  Quaternion<T_Scalar>& Quaternion<T_Scalar>::setFromVectors(const Vector3<T_Scalar>& from, const Vector3<T_Scalar>& to)
  {
    Vector3<T_Scalar> a,b;
    a = from;
    b = to;
    a.normalize();
    b.normalize();
    Vector3<T_Scalar> axis = cross(a,b);
    T_Scalar len = 0;
    axis.normalize(&len);
    if(len)
    {
      T_Scalar cosa = vl::dot(a,b);
      cosa = clamp(cosa, (T_Scalar)-1.0, (T_Scalar)+1.0);
      T_Scalar alpha = acos( cosa );
      setFromAxisAngle(axis, alpha*(T_Scalar)dRAD_TO_DEG);
    }
    else
      setNoRotation();
    return *this;
  }
  //-----------------------------------------------------------------------------
  typedef Quaternion<float>  fquat;
  typedef Quaternion<double> dquat;
  typedef Quaternion<real>   quat;
  //-----------------------------------------------------------------------------
}

#endif