Quaternion.hh

/* 
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003-2009    (see file CONTACT for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.

BIAS is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/


#ifndef __Quaternion_hh__
#define __Quaternion_hh__

#include <bias_config.h>

#include <Base/Math/Vector4.hh>
#include <Base/Math/Vector3.hh>
#include <Base/Geometry/RMatrixBase.hh>
#include <Base/Geometry/QuaternionOperators.hh>

namespace BIAS {

#define QUATERNION_EPSILON 1E-10 // DBL_EPSILON, too small (dherzog)
#define QUATERNION_TYPE double

  // forward declaration to avoid mutual inclusion
  class BIASGeometryBase_EXPORT RMatrixBase;

  /** @class Quaternion
   *  @test tested with TestRotationConversion.cpp
      @ingroup g_geometry
      @brief class for rotation with axis and angle

      Quaternions can be used for rotation around an axis through the
      origin by spcecifing the normal direction vector of the axis and
      an angle.
     
      The rotation for positive angles is clockwise,        
      when looking in direction of the axis.

      Contains qx=[0], qy=[1], qz=[2], qw=[3].

      Not every quaternion describes a rotation.
      Only quaternions of the form:     
    
      qx = x * sin(phi/2) , which is the imaginary part i
     
      qy = y * sin(phi/2)  , which is the imaginary part j
     
      qz = z * sin(phi/2)  , which is the imaginary part k
     
      qw = cos(phi/2),     , which is the real part    

      where (x, y, z) is the normalized direction vector of the axis 
      and phi is the angle of rtoation.
     
      Some usefull quaternions: 
      x            y            z          w          Description
      0            0            0          1          Identity quaternion, 
                                                      no rotation
      1            0            0          0            180' turn around X axis
      0            1            0          0            180' turn around Y axis
      0            0            1          0            180' turn around Z axis
      sqrt(0.5)  0          0          sqrt(0.5)  90' rotation around X axis
      0            sqrt(0.5)    0          sqrt(0.5)  90' rotation around Y axis
      0            0            sqrt(0.5)  sqrt(0.5)  90' rotation around Z axis
      -sqrt(0.5) 0          0          sqrt(0.5)  -90' rotation around X axis
      0          -sqrt(0.5) 0          sqrt(0.5)  -90' rotation around Y axis
      0            0            -sqrt(0.5) sqrt(0.5)    -90' rotation around Z axis

      @author grest 06/2003
  */
  template<class QUAT_TYPE> 
  class BIASGeometryBase_EXPORT Quaternion 
    : public Vector4<QUAT_TYPE>
  {
  public:

    ~Quaternion();

    Quaternion();

    Quaternion(const Vector4<QUAT_TYPE>& q);

    /** copy constructor
        @author Daniel Grest (06/2003) */
    inline Quaternion(const Quaternion<QUAT_TYPE> &q);        
    
    Quaternion(QUAT_TYPE i, QUAT_TYPE j, QUAT_TYPE k,
               QUAT_TYPE r);
   
    inline void SetIdentity();

    /**
     * Sets all parts of quaternion in mathematical order,
     * real part first, then 1.,2. and 3. imaginary part
     */
    inline void SetQuaternion(QUAT_TYPE real, QUAT_TYPE i,
                              QUAT_TYPE j, QUAT_TYPE k);

    /** returns the Inverse rotation Quaternion
     */
    inline Quaternion<QUAT_TYPE> Inverse() const;
    
    /** inverts this, by changing the rotation axis by *=-1 
     */
    inline void Invert();

    /** makes Quaternion-representation uniqe, ensuring vector lies
        in upper (by real part) hemisphere. (inplace)
     */
    inline void MakeUnique();

    /** quaternion multiplication: this= this * quat
     */
    inline void Mult(const Quaternion<QUAT_TYPE> &quat);

    /** quaternion multiplication: res = this * arg
     */
    inline void Mult(const Quaternion<QUAT_TYPE> &arg, 
                     Quaternion<QUAT_TYPE> &res) const;

    /** quaternion multiplication: this = quat * this
     */
    inline void MultLeft(const Quaternion<QUAT_TYPE> &quat);
  
    /** rotates the given Vector qith the quaternion ( q v q* )
        the resulting vector is given in res
        @returns 0 in case of no error
        @author Daniel Grest, June 2003
    */
    inline int MultVec(const Vector3<QUAT_TYPE> &vec, 
                       Vector3<QUAT_TYPE> &res) const;

    /* rets result from MultVec()
    @author herzog 2005-07-15 */
    inline  Vector3<QUAT_TYPE> 
      MultVec(const Vector3<QUAT_TYPE> &vec) const;

    /** Computes this^(scale), which scales the angle from axis/angle-
        representation with 'scale'. This is the same like inter-/extra-
        polating between (0,0,0,1)-quaternion and this!
     */
    Quaternion<QUAT_TYPE> Power(const QUAT_TYPE & scale) const;
 
    /** Linear interpolation between this and given quaternion.
        @note Quaternions are assumed to be unit quaternions! */
    Quaternion<QUAT_TYPE> InterpolateLinear(const Quaternion<QUAT_TYPE> &to,
                                            const QUAT_TYPE & t) const;

    /** Spherical interpolation between this and given quaternion.
        @note Quaternions are assumed to be unit quaternions! */
    Quaternion<QUAT_TYPE> Interpolate(const Quaternion<QUAT_TYPE> &to,
                                      const QUAT_TYPE & t) const;

    /** sets the quaternion with given rotation axis and angle (in rad)
        @returns 0 in case of no error
        @author Daniel Grest, June 2003
    */
    inline void SetValueAsAxisRad(const Vector3<QUAT_TYPE> &axis,
                                  QUAT_TYPE angle);
 
    /** sets the quaternion with given rotation axis and angle (in rad)
        @returns 0 in case of no error
        @author Daniel Grest, June 2003
    */

    inline void SetValueAsAxisRad(QUAT_TYPE axisX, QUAT_TYPE axisY,
                                  QUAT_TYPE axisZ, QUAT_TYPE angle);

    /** Returns matrix which expresses (left) quaternion multiplication.
     * A quaternions stored in an object can be understood as a 4-vector
     * q =(ix iy iz r)^T.
     * Left multiplying a quaternion q2 with a quaternion q1 can be
     * expressed in terms of matrix multiplication as
     * qRes = q1*q2 = M(q1)*q2.
     * This method returns the matrix M(*this).
     */
    Matrix4x4<QUAT_TYPE> GetQuaternionMultMatrixLeft() const;

    /** Returns matrix which expresses right quaternion multiplication.
     * Right multiplying a quaternion q1 with quaternion q2 can be
     * expressed as qRes = q1*q2 = N(q2)*q1.
     * This method returns the matrix N(*this).
     */
    Matrix4x4<QUAT_TYPE> GetQuaternionMultMatrixRight() const;
 
    /** Returns a corresponding (quaternion) rotation matrix.
        @deprecated Transpose problem! This function is going to be removed!
        Use RMatrixBase::SetFromQuaternion() instead!
        @author grest */
    int GetRotationMatrix(RMatrixBase& R) const;

    /** Returns rotation axis. See GetRotationAxisAngle().
     */
    Vector3<QUAT_TYPE> GetRotationAxis() const;    

    /** Returns rotation angle notation in radians.
        Note that the returned angle resides in the interval [0,PI)!
    */
    QUAT_TYPE GetRotationAngle() const;    
  
    /** Returns rotation in axis and angle notation (angle in radians).
        Note that the returned angle resides in the interval [0,PI) and
        the axis points into the according direction!
        @author woelk 12 2002 */
    int GetAxisAngle(Vector3<QUAT_TYPE>& axis, QUAT_TYPE& angle) const;

    /** Sets quaternion as concatenated rotations around 
        x,y,z-axis; this = q_x * q_y * q_z (BIAS-RMatrix conform)
    @author herzog 2005-07-19 */    
    int SetXYZ(QUAT_TYPE radX, QUAT_TYPE radY, QUAT_TYPE radZ);

    /** Sets quaternion as concatenated rotations around 
        x,y,z-axis; this = q_z * q_y * q_x (BIAS-RMatrix conform)
    @author herzog 2005-07-19 */    
    int SetZYX(QUAT_TYPE radX, QUAT_TYPE radY, QUAT_TYPE radZ);


    /** Scales quaternion to unit length, i.e. norm equals 1. */
    inline void Normalize() {
      (*this) /= (QUAT_TYPE)this->NormL2();
    }

    /** assignment operator
        @author grest 06 2003 */
    Quaternion<QUAT_TYPE>& operator=(const Quaternion<QUAT_TYPE>& vec);

    /** @brief Enforce rigid coupling constraint for this and the other given
               unit quaternion (equal rotation angle/equal scalar part for both).
        Computes direct simple interpolation of both unit quaternion where
        the scalar parts of the resulting unit quaternion are identical.
        This solution can be used as initial guess for numerical refinement.
        @param[in/out] other Quaternion to enforce rigid coupling constraint with
        @param[in] useOtherAngle Use rotation angle (= scalar part) of other
                                 quaternion instead of interpolating both
        @author esquivel 02/2012
    */
    void EnforceRigidCouplingConstraint(Quaternion<QUAT_TYPE> &other,
                                        bool useOtherAngle = false);

    /** @brief Enforce rigid coupling constraint for all quaternions in given vector.
        @param[in/out] quats Quaternions to enforce rigid coupling constraint for
        @param[in] useFirstAngle Use rotation angle (= scalar part) of first
                                 quaternion instead of interpolating all
        @author esquivel 02/2012
    */
    static void EnforceRigidCouplingConstraint(
      std::vector< Quaternion<QUAT_TYPE> > &quats, bool useFirstAngle = false);

    /** @brief Compute minimal parametrization of unit quaternion by
               projecting it from the 4d hypersphere onto the 3d equatorial
               space, as described in:
        [TCBS+12] Terzakis et al.: A Recipe on the Parameterization of Rotation
                  Matrices for Non-Linear Optimization using Quaternions, 2012.
        @param[out] p3D Returns 3d point representation of unit quaternion
        @author esquivel 07/2013
    */
    inline void GetEquatorialPoint3D(Vector3<QUAT_TYPE> &p3D) const;
    
    /** @brief Set unit quaternion from 3d point in equatorial space by back-
               projecting it onto the 4d hypersphere, as described in [TCBS+12].
        @param[in] p3D Constaint 3d point representation of unit quaternion
        @see GetEquatorialPoint3D
        @author esquivel 07/2013
    */
    inline void SetFromEquatorialPoint3D(const Vector3<QUAT_TYPE> &p3D);

  }; // class

  #include <Base/Geometry/QuaternionInl.hh>

} // namespace

#endif // __Quaternion_hh__