MeVis/Foundation/Sources/MLLinearAlgebra/mlQuaternion.h

Go to the documentation of this file.

// **InsertLicense** code 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------

#ifndef __mlQuaternion_H
#define __mlQuaternion_H

// Resolves system dependencies and load project settings.
#ifndef __mlLinearAlgebraSystem_H
#include "mlLinearAlgebraSystem.h"
#endif

// 3D double vector.
#ifndef __mlVector3_H
#include "mlVector3.h"
#endif


ML_LA_START_NAMESPACE

//--------------------------------------------------------
//--------------------------------------------------------
template <typename DT>
class TQuaternion {

public:
  //------------------------------------------------------

  //------------------------------------------------------
  union {
    // We use a union to make the members accessible
    // either
    // - as named members qx, qy, qz and qw,
    // - as array[0..3] and
    // - vector (qv,qw) via an access method qv().
    // We also provide indexing operators [] on the array.
    // Note:
    //   The real part qw is stored in the last component.

    //------------------------------------------------------
    //------------------------------------------------------
    struct {
      DT qx;  
      DT qy;  
      DT qz;  
      DT qw;  
    };

    //------------------------------------------------------
    //------------------------------------------------------
    DT array[4];
  };


  //------------------------------------------------------

  //------------------------------------------------------
  inline TQuaternion() :
    qx(0), qy(0), qz(0), qw(0) { }

  inline explicit TQuaternion(DT d) :
    qx(0), qy(0), qz(0), qw(d)
  {
    ML_TRACE_IN_TIME_CRITICAL("TQuaternion(DT d)");
  }

  template <typename DT2>
  inline explicit TQuaternion(const TQuaternion<DT2> &q2)
  {
    ML_TRACE_IN_TIME_CRITICAL("TQuaternion(const TQuaternion<DT2> &q2)");

    qx = static_cast<DT>(q2.qx);
    qy = static_cast<DT>(q2.qy);
    qz = static_cast<DT>(q2.qz);
    qw = static_cast<DT>(q2.qw);
  }

  inline explicit TQuaternion(DT x, DT y, DT z, DT w)
  {
    ML_TRACE_IN_TIME_CRITICAL("TQuaternion(DT x, DT y, DT z, DT w)");

    qx = x;
    qy = y;
    qz = z;
    qw = w;
  }

  inline explicit TQuaternion(const FloatingPointVector<DT,4> &v)
  {
    ML_TRACE_IN_TIME_CRITICAL("TQuaternion(const FloatingPointVector<DT,4> &v)");
    qx = v[1];
    qy = v[2];
    qz = v[3];
    qw = v[0];
  }

  inline explicit TQuaternion(const FloatingPointVector<DT, 3, Vector3DataContainer<DT> > &v, DT w) :
    qx(v[0]), qy(v[1]), qz(v[2]), qw(w)
  {
    ML_TRACE_IN_TIME_CRITICAL("TQuaternion(const FloatingPointVector<DT, 3> &v, DT w)");
  }



  //-------------------------------------------------------------------------

  //-------------------------------------------------------------------------

  inline FloatingPointVector<DT, 3, Vector3DataContainer<DT> > getImaginaryPart() const 
  { 
    return qv(); 
  }

  inline DT getRealPart() const 
  { 
    return qw; 
  }  

  inline const FloatingPointVector<DT, 3, Vector3DataContainer<DT> > &qv() const 
  { 
    return reinterpret_cast<const FloatingPointVector<DT, 3, Vector3DataContainer<DT> >&>(qx); 
  }

  inline FloatingPointVector<DT, 3, Vector3DataContainer<DT> > &qv()
  {
    return reinterpret_cast<FloatingPointVector<DT, 3, Vector3DataContainer<DT> >&>(qx); 
  }

  inline void  set(const FloatingPointVector<DT, 3, Vector3DataContainer<DT> > &v, const DT w)
  {
    ML_TRACE_IN_TIME_CRITICAL("TQuaternion::set(const FloatingPointVector<DT, 3> &v, const DT w)");
    
    qx = v[0];
    qy = v[1];
    qz = v[2];
    qw = w;
  }

  inline void  set(const DT x, const DT y, const DT z, const DT w)
  {
    ML_TRACE_IN_TIME_CRITICAL("TQuaternion::set(const DT x, const DT y, const DT z, const DT w)");
    
    qx = x;
    qy = y;
    qz = z;
    qw = w;
  }

  inline Tmat4<DT> getAsMat4(bool *isConvertible=NULL) const
  {
    ML_TRACE_IN("TQuaternion::getAsMat4() const");

    // Create return value.
    Tmat4<DT> m;
    ML_TRY
    {
      // Check whether conversion is possible.
      const DT n=norm();
      if (0==n) {
        if (!isConvertible){
          ML_PRINT_ERROR("TQuaternion::getAsMat4() const, quaternion is not convertible to 4x4 matrix",
                         ML_BAD_PARAMETER,
                         "Returning Quaterion");
        }
        else{
          *isConvertible = false;
        }
        m = Tmat4<DT>::getIdentity();
      }
      else{
        // Conversion is possible.
        const DT s = 2./n;
        //s is a compressed factor
        //it originates from the constant factor 2 and the normalization
        //factor for the vector "1/sqrt(norm(vector))"
        //To normalize the vector each component is divided by "1/sqrt(norm(vector))"
        //So each vector component qx,qy,qz has to get its own factor to normalize
        //the vector while calculating the matrix. However, as s is multiplied
        //always to a pair of 2 multiplied vector components, the factor can be
        //pulled in front of the addition (c.f. m11). As pairs of vector components
        //are added, the sqrt vanishes and what is left is the factor 2./norm(vector)
        //where norm is what is defined as "norm()" inside the mlQuaternion.h.

        // q0 = qw, first column.
        const DT qxqx = qx*qx;
        const DT qyqy = qy*qy;
        const DT qzqz = qz*qz;

        const DT qxqy = qx*qy;
        const DT qxqz = qx*qz;
        const DT qxqw = qx*qw;

        const DT qyqz = qy*qz;
        const DT qyqw = qy*qw;

        const DT qzqw = qz*qw;

        const DT m11 = 1 - s * (qyqy + qzqz);
        const DT m12 =     s * (qxqy - qzqw);
        const DT m13 =     s * (qxqz + qyqw);

        const DT m21 =     s * (qxqy + qzqw);
        const DT m22 = 1 - s * (qxqx + qzqz);
        const DT m23 =     s * (qyqz - qxqw);

        const DT m31 =     s * (qxqz - qyqw);
        const DT m32 =     s * (qyqz + qxqw);
        const DT m33 = 1 - s * (qxqx + qyqy);

        // Construct matrix from precomputed values
        m.set(0);
        m[0][0] = m11;
        m[0][1] = m12;
        m[0][2] = m13;
        m[0][3] = 0.0;

        m[1][0] = m21;
        m[1][1] = m22;
        m[1][2] = m23;
        m[1][3] = 0.0;

        m[2][0] = m31;
        m[2][1] = m32;
        m[2][2] = m33;
        m[2][3] = 0.0;

        m[3][0] = 0.0;
        m[3][1] = 0.0;
        m[3][2] = 0.0;
        m[3][3] = 1.0;

        if (isConvertible){ *isConvertible = true; }

      } // else

    }
    ML_CATCH_RETHROW;
    return m;
  }



  //-------------------------------------------------------------------------

  //-------------------------------------------------------------------------

  inline DT operator[](const size_t i) const { return array[i]; }

  inline DT &operator[](const size_t i)      { return array[i]; }

  inline TQuaternion<DT> &operator=(const TQuaternion<DT> &q)
  {
    if (this != &q){
      qx = q.qx;
      qy = q.qy;
      qz = q.qz;
      qw = q.qw;
    }
    return *this;
  }


  //------------------------------------------------------

  //------------------------------------------------------

  inline TQuaternion<DT>  operator +   (const TQuaternion<DT> &b) const { return TQuaternion<DT>(qx+b.qx, qy+b.qy, qz+b.qz, qw+b.qw); }
  inline TQuaternion<DT> &operator +=  (const TQuaternion<DT> &b)       { qx+=b.qx; qy+=b.qy; qz+=b.qz; qw+=b.qw; return *this;       }

  inline TQuaternion<DT>  operator -   (const TQuaternion<DT> &b) const { return TQuaternion<DT>(qx-b.qx, qy-b.qy, qz-b.qz, qw-b.qw); }
  inline TQuaternion<DT> &operator -=  (const TQuaternion<DT> &b)       { qx-=b.qx; qy-=b.qy; qz-=b.qz; qw-=b.qw; return *this;       }

  inline TQuaternion<DT>  operator *   (const TQuaternion<DT> &b) const { return mult(b);                                             }
  inline TQuaternion<DT> &operator *=  (const TQuaternion<DT> &b)       { *this = mult(b); return *this;                              }

  inline TQuaternion<DT>  operator /   (DT d) const
  {
    ML_TRACE_IN("TQuaternion::operator/");

    TQuaternion<DT> retVal(*this);
    ML_TRY
    {
      ML_CHECK_FLOAT(d);
      retVal /= d;
    }
    ML_CATCH_RETHROW;
    return retVal;
  }

  inline TQuaternion<DT> &operator /=  (DT d)
  {
    ML_TRACE_IN("TQuaternion::operator/=");
    ML_TRY
    {
      ML_CHECK_FLOAT(d);
      qx/=d; qy/=d; qz/=d; qw/=d;
    }
    ML_CATCH_RETHROW;
    return *this;
  }


  inline TQuaternion<DT>  operator / (const TQuaternion<DT> &q) const
  {
    return TQuaternion<DT>(div(q));
  }

  inline TQuaternion<DT> compDiv(const TQuaternion<DT> &b, bool *isError=NULL) const
  {
    ML_TRACE_IN( "TQuaternion::compDiv/  " );

    // Initialize return value with default quaternion.
    TQuaternion<DT> retVal(0,0,0,1);
    ML_TRY
    {
      if (!isError){
        // No error flag passed. Handle error if necessary.
        ML_CHECK_FLOAT(b.qx);
        ML_CHECK_FLOAT(b.qy);
        ML_CHECK_FLOAT(b.qz);
        ML_CHECK_FLOAT(b.qw);
        retVal.set(qx/b.qx, qy/b.qy, qz/b.qz, qw/b.qw);
      }
      else{
        // Error flag is available. Return *isError as true in case of error
        *isError = (b.qx == 0) ||
                   (b.qy == 0) ||
                   (b.qz == 0) ||
                   (b.qw == 0);

        retVal = *isError ?
                    TQuaternion<DT>(b.qx != 0 ? qx/b.qx : qx,
                                    b.qy != 0 ? qy/b.qy : qy,
                                    b.qz != 0 ? qz/b.qz : qz,
                                    b.qw != 0 ? qw/b.qw : qw) :
                    TQuaternion<DT>(qx/b.qx,
                                    qy/b.qy,
                                    qz/b.qz,
                                    qw/b.qw);
      }
    }
    ML_CATCH_RETHROW;
    return retVal;
  }

  inline bool    operator ==  (const TQuaternion<DT> &b)          const { return MLValuesAreEqualWOM(qx, b.qx) && 
                                                                                 MLValuesAreEqualWOM(qy, b.qy) && 
                                                                                 MLValuesAreEqualWOM(qz, b.qz) && 
                                                                                 MLValuesAreEqualWOM(qw, b.qw); }
  inline bool    operator !=  (const TQuaternion<DT> &b)          const { return !operator==(b); }



  inline DT               dot(const TQuaternion<DT> &p)           const { return qx*p.qx + qy*p.qy + qz*p.qz + qw*p.qw; }

  inline TQuaternion<DT>  odd(const TQuaternion<DT> &p)           const { return TQuaternion<DT>(qy*p.qz - qz*p.qy,
                                                                                                 qz*p.qx - qx*p.qz,
                                                                                                 qx*p.qy - qy*p.qx,
                                                                                                 0); }

  inline TQuaternion<DT>  even(const TQuaternion<DT> &p)          const { return TQuaternion<DT>(qw*p.qx + qx*p.qw,
                                                                                                 qw*p.qy + qy*p.qw,
                                                                                                 qw*p.qz + qz*p.qw,
                                                                                                 qw*p.qw - qx*p.qx - qy*p.qy - qz*p.qz); }

  inline TQuaternion<DT>  mult(const TQuaternion<DT> &q)          const { Tvec3<DT> img(qv()); Tvec3<DT> qimg(q.qv()); return TQuaternion<DT>(img.cross(qimg) + img*q.qw + qw*qimg, qw*q.qw - img.dot(qimg)); }

  inline TQuaternion<DT>  euclideanMult(const TQuaternion<DT> &q) const { return conjugate().mult(q); }

  inline TQuaternion<DT>  outer(const TQuaternion<DT> &q)         const { return (euclideanMult(q) - q.euclideanMult(*this)) / 2; }


  inline TQuaternion<DT> div(const TQuaternion<DT> &d, bool *isError=NULL) const
  {
    ML_TRACE_IN("TQuaternion::div()");

    // Initialize return value with default quaternion.
    TQuaternion<DT> retVal(0,0,0,1);
    ML_TRY
    {
      if (d == TQuaternion<DT>(0,0,0,0)){
        if (isError == NULL){
          // No error flag passed. Handle error if necessary.
          ML_PRINT_ERROR("TQuaternion::div() const, quaternion is not divisible",
                         ML_BAD_PARAMETER,
                         "Returning unchanged quaternion");
          retVal.set(qx,qy,qz,qw);
        }
        else{
          // Error flag pointer is available. Return *isError as true in case of error
          *isError = true;
          retVal.set(qx,qy,qz,qw);
        }
      }
      else{
        if (isError != NULL){ *isError = false; }
        retVal.set((d.qw*qx - d.qx*qw - d.qy*qz + d.qz*qy)/(d.qw*d.qw + d.qx*d.qx + d.qy*d.qy + d.qz*d.qz),
                   (d.qw*qy + d.qx*qz - d.qy*qw - d.qz*qx)/(d.qw*d.qw + d.qx*d.qx + d.qy*d.qy + d.qz*d.qz),
                   (d.qw*qz - d.qx*qy + d.qy*qx - d.qz*qw)/(d.qw*d.qw + d.qx*d.qx + d.qy*d.qy + d.qz*d.qz),
                   (d.qw*qw + d.qx*qx + d.qy*qy + d.qz*qz)/(d.qw*d.qw + d.qx*d.qx + d.qy*d.qy + d.qz*d.qz));
      }
    }
    ML_CATCH_RETHROW;
    return retVal;
  }


  inline TQuaternion<DT>  mult(DT s)                              const { return TQuaternion<DT>(qx*s,  qy*s,  qz*s,  qw*s);             }

  inline TQuaternion<DT>  add(const TQuaternion<DT> &b)           const { return TQuaternion<DT>(qx+b.qx, qy+b.qy, qz+b.qz, qw+b.qw);    }

  inline TQuaternion<DT>  add(DT s)                               const { return TQuaternion<DT>(qx, qy, qz, qw+s);                      }

  inline TQuaternion<DT>  conjugate()                             const { return TQuaternion<DT>(-qx, -qy, -qz, qw);                     }

  inline TQuaternion<DT>  negate()                                const { return TQuaternion<DT>(-qx, -qy, -qz, -qw);                    }

  inline DT               norm()                                  const { return qx*qx + qy*qy + qz*qz + qw*qw;                          }


  inline DT               norm2()                                 const { return static_cast<DT>(::sqrt(qx*qx + qy*qy + qz*qz + qw*qw)); }

  inline DT               absoluteValue()                         const { return static_cast<DT>(::sqrt(norm()));                        }

  inline TQuaternion<DT> normalize(bool *isError=NULL) const
  {
    ML_TRACE_IN("TQuaternion::normalize()");

    // Initialize return value with default quaternion.
    TQuaternion<DT> retVal(0,0,0,1);
    ML_TRY
    {
      const DT n=norm2();
      if (MLValueIs0WOM(n)){
        // Error! Check whether we have a pointer to a return error code value.
        if (isError == NULL){
          // No, post an error.
          ML_PRINT_ERROR("TQuaternion::normalize() const, quaternion can not be normalized",
                         ML_BAD_PARAMETER,
                         "Returning default quaternion");
        }
        else{
          // Yes, set return code.
          *isError = true;
        }
      }
      else{
        // Non zero => no error.
        if (isError){ *isError = false; }
        retVal.set(qx/n, qy/n, qz/n, qw/n);
      }
    }
    ML_CATCH_RETHROW;

    return retVal;
  }

  inline TQuaternion<DT>  arg(bool *isError=NULL) const 
  {
    ML_TRACE_IN("TQuaternion::arg() const");

    // Initialize return value with default quaternion.
    TQuaternion<DT> retVal(0,0,0,1);
    ML_TRY
    {
      // Norm2 must be non-zero to avoid divisions by zero.
      const DT n=norm2();
      if (MLValueIs0WOM(n)){
        // Error! Check whether we have a pointer to a return error code value.
        if (isError == NULL){
          // No, post an error.
          ML_PRINT_ERROR("TQuaternion::arg() const, quaternion can not be normalized",
                         ML_BAD_PARAMETER,
                         "Returning default quaternion");
        }
        else{
          // Yes, set return code.
          *isError = true;
        }
      }
      else{
        // Non zero => no error.
        if (isError != NULL){ *isError = false; }
        retVal = acos(qw/n);
      }
    }
    ML_CATCH_RETHROW;
    return retVal;
  }

  inline TQuaternion<DT> sgn(bool *isError=NULL) const
  {
    ML_TRACE_IN("TQuaternion::sgn()");

    // Initialize return value with default quaternion.
    TQuaternion<DT> retVal(0,0,0,1);
    ML_TRY
    {
      const DT absVal = absoluteValue();
      if (isError != NULL){
        // Set error return value.
        if (absVal==0){
          *isError = true;
        }
        else{
          // No error.
          *isError = false;
          retVal = *this / absVal;
        }
      }
      else{
        // Post error is absVal == 0.
        ML_CHECK_FLOAT(absVal);
        retVal = *this / absVal;
      }
    }
    ML_CATCH_RETHROW;
    return retVal;
  }

  inline TQuaternion<DT> inverse(bool* isInvertible=NULL) const
  {
    ML_TRACE_IN("TQuaternion::inverse()");

    // Initialize return value with default quaternion.
    TQuaternion<DT> retVal(0,0,0,1);
    ML_TRY
    {
      const DT n=norm();
      if (MLValueIs0WOM(n)) {
        if (isInvertible==NULL){
          ML_PRINT_ERROR("TQuaternion::() const, quaternion is not invertable",
                         ML_BAD_PARAMETER,
                         "Returning default quaternion");
        }
        else{
          *isInvertible = false;
        }
        retVal = TQuaternion<DT>();
      }
      else{
        if (isInvertible != NULL){ *isInvertible = true; }
        retVal = conjugate().mult(1/n);
      }
    }
    ML_CATCH_RETHROW;
    return retVal;
  }

  // return TQuaternion(::sqrt(norm2())*::sin(0.5*acos(qw/norm2()))*(1/qv().length())*Tvec3<DT>(qx,qy,qz),
  inline TQuaternion<DT> sqrt(bool *isError=NULL)  const
  {
    ML_TRACE_IN("TQuaternion::sqrt() const");

    // Initialize return value with default quaternion.
    TQuaternion<DT> retVal(0,0,0,1);
    ML_TRY
    {
      // Precalculate constants and check for 0 division.
      const DT n2  = norm2();
      const DT lqv = qv().length();
      if (MLValueIs0WOM(n2) || MLValueIs0WOM(lqv)){
        // Error! Check whether we have a pointer to a return error code value.
        if (isError == NULL){
          // No, post an error.
          ML_PRINT_ERROR("TQuaternion::sqrt() const, sqrt of quaternion cannot be calculated",
                         ML_BAD_PARAMETER,
                         "Returning default quaternion");
        }
        else{
          // Yes, set return code.
          *isError = true;
        }
      }
      else{
        // Calculate sqrt, lqv and n2 are non 0 here.
        const DT sqrtSin  = ::sqrt(n2)*::sin(0.5*acos(qw/n2));
        const DT sqrtSinF = sqrtSin*(1/lqv);
        retVal.set(sqrtSinF*qx, sqrtSinF*qy, sqrtSinF*qz, ::sqrt(n2) * ::cos(0.5*acos(qw/n2)));

        // Non zero => no error.
        if (isError != NULL){ *isError = false; }
      }
    }
    ML_CATCH_RETHROW;
    return retVal;
  }



  inline TQuaternion<DT> exp() const
  {
    ML_TRACE_IN("TQuaternion::exp() const");

    // Initialize return value with default quaternion.
    TQuaternion<DT> retVal(0,0,0,1);
    ML_TRY
    {
      const DT len = qv().length();
      if (MLValueIs0WOM(len)){
        retVal = TQuaternion<DT>(0,0,0,::cos(len)).mult(::exp(qw));
      }
      else{
        retVal = TQuaternion<DT>((qv()/len) * static_cast<DT>(::sin(len)), ::cos(len)).mult(::exp(qw));
      }
    }
    ML_CATCH_RETHROW;
    return retVal;
  }


  inline TQuaternion<DT> ln() const
  {
    ML_TRACE_IN("TQuaternion::exp() const");

    // Initialize return value with default quaternion.
    TQuaternion<DT> retVal(0,0,0,1);
    ML_TRY
    {
      const DT n2    = norm2();
      const DT qvLen = qv().length();
      if (MLValueIs0WOM(qvLen)){
        retVal.set(0, 0, 0, log10(n2));
      }
      else{
        // n2 is always non 0 if qvLen is non-zero, thus no check is necessary.
        retVal.set(qv()/qvLen * static_cast<DT>(acos(qw/n2)), log10(n2));
      }
    }
    ML_CATCH_RETHROW;
    return retVal;
  }


  inline TQuaternion<DT> pow(const TQuaternion<DT> &quat)      const {return ln().mult(quat).exp(); }



  TQuaternion<DT> sin() const
  {
    ML_TRACE_IN("TQuaternion<DT>::sinh() const");

    TQuaternion<DT> retVal;
    ML_TRY
    {
      const Tvec3<DT> &v  = qv();
      const DT        len = v.length();
      // In case of len == 0 be sure not to divide by zero but setting vector to 0 vector.
      // Then the quaternion method behaves like the normal function on real values.
      retVal.set((!MLValueIs0WOM(len) ?
                    static_cast<DT>(::cos(qw)) * v / len * static_cast<DT>(::sinh(len)) :
                    Tvec3<DT>(0.0)), 
                 ::sin(qw)*::cosh(len)
                );
    }
    ML_CATCH_RETHROW;
    return retVal;
  }

  TQuaternion<DT> cos() const
  {
    ML_TRACE_IN("TQuaternion<DT>::sinh() const");

    TQuaternion<DT> retVal;
    ML_TRY
    {
      const Tvec3<DT> &v  = qv();
      const DT        len = v.length();
      // In case of len == 0 be sure not to divide by zero but setting vector to 0 vector.
      // Then the quaternion method behaves like the normal function on real values.
      retVal.set((!MLValueIs0WOM(len) ? 
                    static_cast<DT>(-1)*static_cast<DT>(::sin(qw))*qv()/len*static_cast<DT>(::sinh(len)) :
                    Tvec3<DT>(0.0)), 
                 ::cos(qw)*::cosh(len)
                );

    }
    ML_CATCH_RETHROW;
    return retVal;
  }

  inline TQuaternion<DT> tan(bool *isError=NULL)    const { return sin().div(cos(), isError);  }

  inline TQuaternion<DT> cotan(bool *isError=NULL)  const { return cos().div(sin(), isError);  }




  TQuaternion<DT> sinh() const
  {
    ML_TRACE_IN("TQuaternion<DT>::sinh() const");

    TQuaternion<DT> retVal;
    ML_TRY
    {
      const Tvec3<DT> &v  = qv();
      const DT        len = v.length();
      // In case of len == 0 be sure not to divide by zero but setting vector to 0 vector.
      // Then the quaternion method behaves like the normal function on real values.
      retVal.set((!MLValueIs0WOM(len) ? 
                     static_cast<DT>(::cosh(qw))*v/len*static_cast<DT>(::sin(len)) :
                     Tvec3<DT>(0.0)), 
                 ::sinh(qw)*::cos(len)
                );
    }
    ML_CATCH_RETHROW;
    return retVal;
  }

  TQuaternion<DT> cosh() const
  {
    ML_TRACE_IN("TQuaternion<DT>::cosh() const");

    TQuaternion<DT> retVal;
    ML_TRY
    {
      const Tvec3<DT> &v  = qv();
      const DT        len = v.length();
      // In case of len == 0 be sure not to divide by zero but setting vector to 0 vector.
      // Then the quaternion method behaves like the normal function on real values.
      retVal.set((!MLValueIs0WOM(len) ? 
                     static_cast<DT>(::sinh(qw))*v/len*static_cast<DT>(::sin(len)) :
                     Tvec3<DT>(0.0)), 
                 ::cosh(qw)*::cos(len)
                );
    }
    ML_CATCH_RETHROW;
    return retVal;
  }

  inline TQuaternion<DT> tanh(bool *isError=NULL) const
  {
    ML_TRACE_IN("TQuaternion<DT>::tanh(bool *isError=NULL) const");

    TQuaternion<DT> retVal;
    ML_TRY
    {
      retVal = sinh().div(cosh(), isError);
    }
    ML_CATCH_RETHROW;
    return retVal;
  }

  inline TQuaternion<DT> cotanh(bool *isError=NULL) const
  { 
    ML_TRACE_IN("TQuaternion<DT>::cotanh(bool *isError=NULL) const");

    TQuaternion<DT> retVal;
    ML_TRY
    {
      retVal = cosh().div(sinh(), isError);
    }
    ML_CATCH_RETHROW;
    return retVal;
  }




  inline TQuaternion<DT> arcsinh()                             const { return add(mult(*this).add(TQuaternion<DT>( 1)).sqrt()).ln(); }

  inline TQuaternion<DT> arccosh()                             const { return add(mult(*this).add(TQuaternion<DT>(-1)).sqrt()).ln(); }

  inline TQuaternion<DT> arctanh()                             const { return TQuaternion<DT>((TQuaternion<DT>(qx,qy,qz,qw+1).ln()-TQuaternion<DT>(-qx, -qy, -qz, 1-qw).ln())/0.5);                                                   }


  #define _ML_QUATERNION_CALC_CHECKED(FUNC_NAME, CALC_EXPR)                          \
      ML_TRACE_IN("TQuaternion<DT>::" FUNC_NAME "() const");                         \
                                                                                     \
      TQuaternion<DT> retVal;                                                        \
      ML_TRY                                                                         \
      {                                                                              \
        const Tvec3<DT> &v  = qv();                                                  \
        const DT        len = v.length();                                            \
        if (MLValueIs0WOM(len)){                                                     \
          /* Error! Check whether we have a pointer to a return error code value. */ \
          if (isError == NULL){                                                      \
            /* No, post an error. */                                                 \
            ML_PRINT_ERROR("TQuaternion::" FUNC_NAME "() const, " FUNC_NAME          \
                           " of quaternion cannot be calculated",                    \
                           ML_BAD_PARAMETER,                                         \
                           "Returning default quaternion");                          \
          }                                                                          \
          else{                                                                      \
            /* Yes, set return code, retVal is left on default. */                   \
            *isError = true;                                                         \
          }                                                                          \
        }                                                                            \
        else{                                                                        \
          /* Define return values. */                                                \
          if (isError != NULL){ *isError = false; }                                  \
          retVal = CALC_EXPR;                                                        \
        }                                                                            \
      }                                                                              \
      ML_CATCH_RETHROW;                                                              \
      return retVal;                                                                 \



  #define _ML_QUAT_CALC_EXP TQuaternion<DT>(-v/len,0).mult(mult(TQuaternion<DT>(-v/len,0)).arcsinh());





  TQuaternion<DT> arcsin(bool *isError=NULL) const
  {
    _ML_QUATERNION_CALC_CHECKED("arcsin", _ML_QUAT_CALC_EXP);
  }
  #undef _ML_QUAT_CALC_EXP



  #define _ML_QUAT_CALC_EXP TQuaternion<DT>(-v/len,0).mult(arccosh());

  TQuaternion<DT> arccos(bool *isError=NULL) const
  {
    _ML_QUATERNION_CALC_CHECKED("arccos", _ML_QUAT_CALC_EXP);
  }
  #undef _ML_QUAT_CALC_EXP



  #define _ML_QUAT_CALC_EXP TQuaternion<DT>(-v/len,0).mult(mult(TQuaternion<DT>(-v/len,0)).arctanh());

  TQuaternion<DT> arctan(bool *isError=NULL) const
  {
    _ML_QUATERNION_CALC_CHECKED("arctan", _ML_QUAT_CALC_EXP);
  }
  #undef _ML_QUAT_CALC_EXP


  // Undefine _ML_QUATERNION_CALC_CHECKED, it is not needed any more.
  #undef _ML_QUATERNION_CALC_CHECKED
};


//-----------------------------------------------------------------------------------

//-----------------------------------------------------------------------------------
typedef TQuaternion<MLfloat>   Quaternionf;

typedef TQuaternion<MLdouble>  Quaterniond;

typedef TQuaternion<MLldouble> Quaternionld;

typedef TQuaternion<MLdouble>  Quaternion;


ML_LA_END_NAMESPACE




//-------------------------------------------------------------------------





//-------------------------------------------------------------------------
#ifdef  _ML_IMPLEMENT_GLOBAL_QUATERNION_OPERATORS
#ifndef _ML_IMPLEMENT_GLOBAL_QUATERNION_FUNCS_AND_OPS_IN_GLOBAL_NAMESPACE
ML_LA_START_NAMESPACE
#endif

//-----------------------------
// Addition
//-----------------------------
template <typename DT>
inline ML_LA_NAMESPACE::TQuaternion<DT> operator+(DT d, const ML_LA_NAMESPACE::TQuaternion<DT> &q){
  return ML_LA_NAMESPACE::TQuaternion<DT>(q.qx, q.qy, q.qz, q.qw+d);
}

template <typename DT>
inline ML_LA_NAMESPACE::TQuaternion<DT> operator+(const ML_LA_NAMESPACE::TQuaternion<DT> &q, DT d){
  return ML_LA_NAMESPACE::TQuaternion<DT>(q.qx, q.qy, q.qz, q.qw+d);
}

template <typename DT>
inline ML_LA_NAMESPACE::TQuaternion<DT> operator+(const ML_LA_NAMESPACE::TQuaternion<DT> &qa,
                                                  const ML_LA_NAMESPACE::TQuaternion<DT> &qb){
  return qa.operator+(qb);
}



//-----------------------------
// Subtraction
//-----------------------------
template <typename DT>
inline ML_LA_NAMESPACE::TQuaternion<DT> operator-(DT d, const ML_LA_NAMESPACE::TQuaternion<DT> &q){
  return ML_LA_NAMESPACE::TQuaternion<DT>(-q.qx, -q.qy, -q.qz, d-q.qw);
}

template <typename DT>
inline ML_LA_NAMESPACE::TQuaternion<DT> operator-(const ML_LA_NAMESPACE::TQuaternion<DT> &q, DT d){
  return ML_LA_NAMESPACE::TQuaternion<DT>(q.qx, q.qy, q.qz, q.qw-d);
}

template <typename DT>
inline ML_LA_NAMESPACE::TQuaternion<DT> operator-(const ML_LA_NAMESPACE::TQuaternion<DT> &qa,
                                                  const ML_LA_NAMESPACE::TQuaternion<DT> &qb){
  return qa.operator-(qb);
}


template <typename DT>
inline ML_LA_NAMESPACE::TQuaternion<DT> operator*(DT d, const ML_LA_NAMESPACE::TQuaternion<DT> &q){
  return q.mult(d);
}

template <typename DT>
inline ML_LA_NAMESPACE::TQuaternion<DT> operator*(const ML_LA_NAMESPACE::TQuaternion<DT> &q, DT d){
  return q.mult(d);
}

template <typename DT>
inline ML_LA_NAMESPACE::TQuaternion<DT> operator*(const ML_LA_NAMESPACE::TQuaternion<DT> &qa,
                                                  const ML_LA_NAMESPACE::TQuaternion<DT> &qb){
  return qa.operator*(qb);
}

#ifndef _ML_IMPLEMENT_GLOBAL_QUATERNION_FUNCS_AND_OPS_IN_GLOBAL_NAMESPACE
ML_LA_END_NAMESPACE
#endif

#endif  // _ML_IMPLEMENT_GLOBAL_QUATERNION_OPERATORS



//-------------------------------------------------------------------------





//-------------------------------------------------------------------------
#ifdef  _ML_IMPLEMENT_GLOBAL_QUATERNION_FUNCTIONS
#ifndef _ML_IMPLEMENT_GLOBAL_QUATERNION_FUNCS_AND_OPS_IN_GLOBAL_NAMESPACE
ML_LA_START_NAMESPACE
#endif

#ifdef _ML_IMPLEMENT_QUATERNION_SQRT
template <typename DT>
inline ML_LA_NAMESPACE::TQuaternion<DT> sqrt   (const ML_LA_NAMESPACE::TQuaternion<DT> &q) { return q.sqrt();    }
#endif

template <typename DT>
inline ML_LA_NAMESPACE::TQuaternion<DT> exp    (const ML_LA_NAMESPACE::TQuaternion<DT> &q) { return q.exp();     }

template <typename DT>
inline ML_LA_NAMESPACE::TQuaternion<DT> ln     (const ML_LA_NAMESPACE::TQuaternion<DT> &q) { return q.ln();      }

template <typename DT>
inline ML_LA_NAMESPACE::TQuaternion<DT> sin    (const ML_LA_NAMESPACE::TQuaternion<DT> &q) { return q.sin();     }

template <typename DT>
inline ML_LA_NAMESPACE::TQuaternion<DT> cos    (const ML_LA_NAMESPACE::TQuaternion<DT> &q) { return q.cos();     }

template <typename DT>
inline ML_LA_NAMESPACE::TQuaternion<DT> tan    (const ML_LA_NAMESPACE::TQuaternion<DT> &q) { return q.tan();     }

template <typename DT>
inline ML_LA_NAMESPACE::TQuaternion<DT> cotan  (const ML_LA_NAMESPACE::TQuaternion<DT> &q) { return q.cotan();   }

template <typename DT>
inline ML_LA_NAMESPACE::TQuaternion<DT> sinh   (const ML_LA_NAMESPACE::TQuaternion<DT> &q) { return q.sinh();    }

template <typename DT>
inline ML_LA_NAMESPACE::TQuaternion<DT> cosh   (const ML_LA_NAMESPACE::TQuaternion<DT> &q) { return q.cosh();    }

template <typename DT>
inline ML_LA_NAMESPACE::TQuaternion<DT> tanh   (const ML_LA_NAMESPACE::TQuaternion<DT> &q) { return q.tanh();    }

template <typename DT>
inline ML_LA_NAMESPACE::TQuaternion<DT> cotanh (const ML_LA_NAMESPACE::TQuaternion<DT> &q) { return q.cotanh();  }

template <typename DT>
inline ML_LA_NAMESPACE::TQuaternion<DT> arcsinh(const ML_LA_NAMESPACE::TQuaternion<DT> &q) { return q.arcsinh(); }

template <typename DT>
inline ML_LA_NAMESPACE::TQuaternion<DT> arccosh(const ML_LA_NAMESPACE::TQuaternion<DT> &q) { return q.arccosh(); }

template <typename DT>
inline ML_LA_NAMESPACE::TQuaternion<DT> arctanh(const ML_LA_NAMESPACE::TQuaternion<DT> &q) { return q.arctanh(); }

template <typename DT>
inline ML_LA_NAMESPACE::TQuaternion<DT> arcsin (const ML_LA_NAMESPACE::TQuaternion<DT> &q) { return q.arcsin();  }

template <typename DT>
inline ML_LA_NAMESPACE::TQuaternion<DT> arccos (const ML_LA_NAMESPACE::TQuaternion<DT> &q) { return q.arccos();  }

template <typename DT>
inline ML_LA_NAMESPACE::TQuaternion<DT> arctan (const ML_LA_NAMESPACE::TQuaternion<DT> &q) { return q.arctan();  }


#ifndef _ML_IMPLEMENT_GLOBAL_QUATERNION_FUNCS_AND_OPS_IN_GLOBAL_NAMESPACE
ML_LA_END_NAMESPACE
#endif

#endif // _ML_IMPLEMENT_GLOBAL_QUATERNION_FUNCTIONS



//-----------------------------------------------------------------------------------
// Stream output for std::ostream and dyadic multiplication of TQuaternion and scalar.
//-----------------------------------------------------------------------------------
namespace std {

  template <typename DT>
  inline ostream& operator<<(ostream& s, const ML_NAMESPACE::TQuaternion<DT> &v){
    return s << "(" << v.qx << "," << v.qy << "," << v.qz << "," << v.qw << ")";
  };

}


#endif // __mlQuaternion_H