RMatrixBase.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 __RMatrixBase_hh__
#define __RMatrixBase_hh__
#include "bias_config.h" 

#include <Base/Math/Vector3.hh>
#include <Base/Math/Matrix3x3.hh>
#include <Base/Debug/Debug.hh>

namespace BIAS {

#define ROTATION_MATRIX_TYPE double
 
#define ROTATION_MATRIX_EPSILON DBL_EPSILON

  template<class T> class BIASGeometryBase_EXPORT Quaternion;

  /** @class RMatrixBase 
      @ingroup g_geometry
      @brief Implements a 3D rotation matrix.
      @author frahm, woelk
  */
  class BIASGeometryBase_EXPORT RMatrixBase 
    : public Matrix3x3<ROTATION_MATRIX_TYPE> 
  {
  public:

    /** @brief Constructor setting matrix to identity */
    RMatrixBase();

    /** @brief Constructor setting matrix to identity or zero
        @deprecated Only identity is allowed here!
        @author koeser
    */
    explicit RMatrixBase(const MatrixInitType& i);

    /** @brief Copy constructor from rotation matrix */
    RMatrixBase(const RMatrixBase& r);

    /** @brief Copy constructor from 3x3 matrix
        @attention Orthonormality of matrix is enforced automatically! */
    RMatrixBase(const Matrix3x3<ROTATION_MATRIX_TYPE>& mat);

    /** @brief Initialization from rotation axis w and angle phi (in rad)
               using Rodrigues' formula */
    RMatrixBase(const Vector3<ROTATION_MATRIX_TYPE> w, 
                const ROTATION_MATRIX_TYPE phi);

    /** @brief Copy constructor from 3x3 matrix
        @attention Orthonormality of matrix is enforced automatically! */
    explicit RMatrixBase(const TNT::Matrix<ROTATION_MATRIX_TYPE>& r);

    virtual ~RMatrixBase();

    /** @brief Set Euler angles (in rad) in order XYZ

        Set angles either in order 1. x, 2. y, 3. z and moving axes 
        or in order 1. z, 2. y, 3. x and fixed axes.
        Angles are measured as in mathematics (counter-clockwise), i.e.
        Set(x, 0, 0) results in the following matrix:

        | 1  0        0      |
        | 0  cos(x)  -sin(x) |
        | 0  sin(x)   cos(x) |
        
        (*this) = Rx*Ry*Rz = 

        |  c(y)c(z)               -c(y)s(z)                s(y)     |
        |  c(z)s(x)s(y)+c(x)s(z)   c(x)c(z)-s(x)s(y)s(z)  -c(y)s(x) |
        | -c(x)c(z)s(y)+s(x)s(z)   c(z)s(x)+c(x)s(y)s(z)   c(x)c(y) |

        with s(g) = sin(g), c(g) = cos(g), x = PhiX, y = PhiY and z = PhiZ

        @author frahm, woelk
     */
    void SetXYZ(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY, 
                ROTATION_MATRIX_TYPE PhiZ);

    /** @brief Set Euler angles (in rad) in order XYZ from vector */
    inline void SetXYZ(const Vector3<ROTATION_MATRIX_TYPE>& r)
    { SetXYZ(r[0], r[1], r[2]); }

    /** @brief Set Euler angles (in rad) in order ZYX

        Set angles either in order 1. z, 2. y, 3. x and moving axes 
        or in order 1. x, 2. y, 3. z and fixed axes.
        Angles are measured as in mathematics (counter-clockwise), i.e.
        Set(x, 0, 0) results in the following matrix:

        | 1  0        0      |
        | 0  cos(x)  -sin(x) |
        | 0  sin(x)   cos(x) |
        
        (*this) = Rz*Ry*Rx = 

        |  c(y)c(z)  s(x)s(y)c(z)-c(x)s(z)  c(x)s(y)c(z)+s(x)s(z) |
        |  c(y)s(z)  s(x)s(y)s(z)+c(x)c(z)  c(x)s(y)s(z)-s(x)s(z) |
        | -s(y)      s(x)c(y)               c(x)c(y)              |

        with s(g) = sin(g), c(g) = cos(g), x = PhiX, y = PhiY and z = PhiZ

        @author frahm, woelk
     */
    void SetZYX(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY, 
                ROTATION_MATRIX_TYPE PhiZ);

    /** @brief Set Euler angles (in rad) in order ZYX from vector */
    inline void SetZYX(const Vector3<ROTATION_MATRIX_TYPE>& r)
    { SetZYX(r[0], r[1], r[2]); }
 
    /** @brief Set Euler angles (in rad) in order ZXY

        Set angles either in order 1. z, 2. x, 3. y and moving axes 
        or in order 1. y, 2. x, 3. z and fixed axes.
        Angles are measured as in mathematics (counter-clockwise), i.e.
        Set(x, 0, 0) results in the following matrix:

        | 1  0        0      |
        | 0  cos(x)  -sin(x) |
        | 0  sin(x)   cos(x) |
        
        (*this) = Rz*Rx*Ry = 

        | c(y)c(z)-s(x)s(y)s(z), c(x)s(z), s(y)c(z)+s(x)c(y)s(z) |
        | s(z)c(y)+s(x)s(y)c(z), c(x)c(z),  s(y)*s(z)+s(x)-c(y)c(z) |
        | -c(x)s(y)             ,  s(x)  ,  c(x)c(y)              |
        
        with s(g) = sin(g), c(g) = cos(g), x = PhiX, y = PhiY and z = PhiZ

        @author haase
     */
    void SetZXY(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY, 
                ROTATION_MATRIX_TYPE PhiZ);

    /** @brief Set Euler angles (in rad) in order ZXY from vector */
    inline void SetZXY(const Vector3<ROTATION_MATRIX_TYPE>& r)
    { SetZXY(r[0], r[1], r[2]); }

     /** @brief Set Euler angles (in rad) in order YXZ
         
         Set angles either in order 1. y, 2. x, 3. z and moving axes 
         or in order 1. z, 2. x, 3. y and fixed axes.
         Angles are measured as in mathematics (counter-clockwise), i.e.
         Set(x, 0, 0) results in the following matrix:

         | 1  0        0      |
         | 0  cos(x)  -sin(x) |
         | 0  sin(x)   cos(x) |
         
         (*this) = Ry*Rx*Rz

         @author haase
     */
    void SetYXZ(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY, 
                ROTATION_MATRIX_TYPE PhiZ);

    /** @brief Set Euler angles (in rad) in order YXZ from vector */
    inline void SetYXZ(const Vector3<ROTATION_MATRIX_TYPE>& r)
    { SetYXZ(r[0], r[1], r[2]); }

    /** @brief Set Euler angles (in rad) in order XZY

        Set angles either in order 1. x, 2. z, 3. y and moving axes 
        or in order 1. y, 2. z, 3. x and fixed axes.

        @author esquivel
     */
    void SetXZY(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY, 
                ROTATION_MATRIX_TYPE PhiZ);

     /** @brief Set Euler angles (in rad) in order XZY from vector */
    inline void SetXZY(const Vector3<ROTATION_MATRIX_TYPE>& r)
    { SetXZY(r[0], r[1], r[2]); }

    /** @brief Set Euler angles (in rad) in order YZX

        Set angles either in order 1. y, 2. z, 3. x and moving axes 
        or in order 1. x, 2. z, 3. y and fixed axes.

        @author esquivel

     */
    void SetYZX(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY, 
                ROTATION_MATRIX_TYPE PhiZ);

    /** @brief Set Euler angles (in rad) in order YZX from vector */
    inline void SetYZX(const Vector3<ROTATION_MATRIX_TYPE>& r)
    { SetYZX(r[0], r[1], r[2]); }
    
    /** @brief Set from rotation axis w and angle phi (in rad)
        @param w Axis vector w will be normalized to length 1, so we need
                 |w|>1e-6 if phi != 0, otherwise an exception is thrown
        @param phi Rotation angle is given in radians
        @author evers, woelk */
    void Set(const Vector3<ROTATION_MATRIX_TYPE> &w, 
             const ROTATION_MATRIX_TYPE phi);

    /** @brief Set from rotation axis * angle (modified Rodrigues vector)
        @author evers */
    void SetFromAxisAngle(Vector3<ROTATION_MATRIX_TYPE> w);

    /* @brief Set rotation matrix from an orthogonal basis given in world
       coordinate system (WCS)
       
       As R' is expressing the transformation from WCS to ICS (image
       coordinate system), R is ICS to WCS and hereby represents the ICS
       base vectors expressed in WCS. These are the *rows* of R because
       the transformation is the scalar product.        
       
       Assume ex,ey,ez are right-hand orthogonal (RHS), e.g. the orthogonal
       image coordinate system (ICS) base vectors expressed in world 
       coordinates (WCS).
       (Inverse doesn't make sense because base would be identity then).
       Normal vectors of length 1 are computed.
       
       @todo Warning if (ex,ey,ez) are not an orthogonal basis!
       
       @author jw 09/2003, added exception
     */
    void SetFromOrthogonalBasis(const BIAS::Vector3<ROTATION_MATRIX_TYPE> &ex,
                                const BIAS::Vector3<ROTATION_MATRIX_TYPE> &ey,
                                const BIAS::Vector3<ROTATION_MATRIX_TYPE> &ez);
    
    /** @brief Set rotation matrix from two vectors from an orthonormal basis
        @param xh represents the first base vector and is left unchanged
        @param vy should be orthogonal to xh, it is orthogonalized otherwise
 
        You can think of this routine as computing R from an image plane
        given by two (usually orthogonal) base vectors.
        If the given base vectors are not orthogonal, xh is kept and yv is 
        orthogonalized appropriately.

        @author jw
     */
    void SetFromHV(const BIAS::Vector3<ROTATION_MATRIX_TYPE> &xh,
           const BIAS::Vector3<ROTATION_MATRIX_TYPE> &vy);
    
    /** @brief Calculates quaternion representation for this rotation matrix
        @attention Scalar part of quaternion will always be non-negative
                   for sake of uniqueness of the resulting quaternion!
        @author woelk 12/2003
        @author esquivel 03/2011 (changed algorithm, bugfix) */
    int GetQuaternion(Quaternion<ROTATION_MATRIX_TYPE> &quat) const ;
    
    /** @brief Set rotation matrix from a quaternion
        @author grest 06/2003 */
    int SetFromQuaternion(const Quaternion<ROTATION_MATRIX_TYPE> &q);
    
    /** @brief Set rotation matrix from orientation and up vector
        @note This is openGL conform, similar to gluLookAt(), i.e. if
        ori is (0,0,-1) and up is (0,1,0) the resulting matrix is identity.
        @author grest 12/2005 */
    int SetFromOriUpGL(BIAS::Vector3<ROTATION_MATRIX_TYPE> ori,
                       BIAS::Vector3<ROTATION_MATRIX_TYPE> up);

    /** @brief Set rotation matrix from orientation and up vector*/
    int SetFromOriUp(BIAS::Vector3<ROTATION_MATRIX_TYPE> ori,
                     BIAS::Vector3<ROTATION_MATRIX_TYPE> up);

    /** @brief Calculates angle and rotation axis representation for
               this rotation matrix
        @param angle Rotation angle is returned in radians
        @author woelk 12/2003 */
    int GetRotationAxisAngle(Vector3<ROTATION_MATRIX_TYPE>& axis, 
                             ROTATION_MATRIX_TYPE& angle) const;

    /** @brief Interface for axis component of GetRotationAxisAngle() */
    BIAS::Vector3<ROTATION_MATRIX_TYPE> GetRotationAxis() const;

    /** @brief Interface for angle component of GetRotationAxisAngle() */
    ROTATION_MATRIX_TYPE GetRotationAngle() const;

    /** @brief Calculates the angle * rotation axis representation for
               this rotation matrix (modified Rodrigues vector)
        @author evers */
    int GetRotationAxisAngle(Vector3<ROTATION_MATRIX_TYPE>& w) const;

    /** @brief Get Euler angles for this rotation matrix in order XYZ
        @attention Representation is not unique and has singularities at
        +-pi/2. Assume for example (*this) = Rx * Ry * Rz, then we have
        either Euler angles in order 1. x, 2. y, 3. z and moving axes
        or in order 1. z, 2. y, 3. x and fixed axes.
        @author woelk 01/2003
     */
    int GetRotationAnglesXYZ(double& PhiX, double& PhiY, double& PhiZ) const;

    /** @brief Get Euler angles for this rotation matrix in order XYZ
        @see GetRotationAnglesXYZ(double&, double&, double&) */
    inline int GetRotationAnglesXYZ(Vector3<ROTATION_MATRIX_TYPE>& r) const
    { return GetRotationAnglesXYZ(r[0], r[1], r[2]); }

    /** @brief Get Euler angles for this rotation matrix in order ZYX
        @attention Representation is not unique and has singularities at
        +-pi/2. Assume for example (*this) = Rz * Ry * Rx, then we have
        either Euler angles in order 1. z, 2. y, 3. x and moving axes
        or in order 1. x, 2. y, 3. z and fixed axes.
        @author woelk 01/2003
     */
    int GetRotationAnglesZYX(double& PhiX, double& PhiY, double& PhiZ) const;

    /** @brief Get Euler angles for this rotation matrix in order ZYX
        @see GetRotationAnglesZYX(double&, double&, double&) */
    inline int GetRotationAnglesZYX(Vector3<ROTATION_MATRIX_TYPE>& r) const
    { return GetRotationAnglesZYX(r[0], r[1], r[2]); }

    /** @brief Get Euler angles for this rotation matrix in order ZXY
        @attention Representation is not unique and has singularities at
        +-pi/2. Rotation order is 1. z, 2. x, 3. y with moving axes or
        1. y, 2. x, 3. z with fixed axes.
        @author haase 2007
     */
    int GetRotationAnglesZXY(double& PhiX, double& PhiY, double& PhiZ) const;

    /** @brief Get Euler angles for this rotation matrix in order ZXY
        @see GetRotationAnglesZXY(double&, double&, double&) */
    inline int GetRotationAnglesZXY(Vector3<ROTATION_MATRIX_TYPE>& r) const
    { return GetRotationAnglesZXY(r[0], r[1], r[2]); }

    /**  @brief Get Euler angles for this rotation matrix in order YXZ
         @attention Representation is not unique and has singularities at
         +-pi/2. Rotation order is 1. y, 2. x, 3. z with moving axes or
         1. z, 2. x, 3. y with fixed axes.
        @author haase 2007 */
    int GetRotationAnglesYXZ(double& PhiX, double& PhiY, double& PhiZ) const;

    /** @brief Get Euler angles for this rotation matrix in order YXZ
        @see GetRotationAnglesYXZ(double&, double&, double&) */
    inline int GetRotationAnglesYXZ(Vector3<ROTATION_MATRIX_TYPE>& r) const
    { return GetRotationAnglesYXZ(r[0], r[1], r[2]); }

    /**  @brief Get Euler angles for this rotation matrix in order YZX
         @attention Representation is not unique and has singularities at
         +-pi/2. Rotation order is 1. y, 2. z, 3. x with moving axes or
         1. x, 2. z, 3. y with fixed axes.
        @author esquivel 2014 */
    int GetRotationAnglesYZX(double& PhiX, double& PhiY, double& PhiZ) const;

    /** @brief Get Euler angles for this rotation matrix in order YZX
        @see GetRotationAnglesYZX(double&, double&, double&) */
    inline int GetRotationAnglesYZX(Vector3<ROTATION_MATRIX_TYPE>& r) const
    { return GetRotationAnglesYZX(r[0], r[1], r[2]); }

    /**  @brief Get Euler angles for this rotation matrix in order XZY
         @attention Representation is not unique and has singularities at
         +-pi/2. Rotation order is 1. x, 2. z, 3. y with moving axes or
         1. y, 2. z, 3. x with fixed axes.
        @author esquivel 2014 */
    int GetRotationAnglesXZY(double& PhiX, double& PhiY, double& PhiZ) const;

    /** @brief Get Euler angles for this rotation matrix in order XZY
        @see GetRotationAnglesXZY(double&, double&, double&) */
    inline int GetRotationAnglesXZY(Vector3<ROTATION_MATRIX_TYPE>& r) const
    { return GetRotationAnglesXZY(r[0], r[1], r[2]); }

    // DLL HACK (jw)
    friend bool BIASGeometryBase_EXPORT 
      operator==(const RMatrixBase& left, const RMatrixBase& right);
  
    /** @brief Check if this is a rotation matrix, i.e. if the determinant
               is +1 and the columns are orthonormal
        @param eps Numerical limit for constraint evaluation
        @param verbose Show reason in case of failure */
    bool Check(const double eps = 
               std::numeric_limits<double>::epsilon(),
               const bool verbose = false) const;

    /** @brief Return the reason for the constraint check failure in a
               human readable string
        @param eps Numerical limit for constraint evaluation
        @return empty string when check succeeds, error message otherwise */
    std::string GetCheckFailureReason(const double eps = 
        std::numeric_limits<double>::epsilon()) const;

    /** @brief Enforce orthonormality constraint on rotation matrix and
               sets determinant to +1 */
    virtual void EnforceConstraints();
    
    /** @brief Setting to zero is forbidden since the zero matrix is not
        a valid rotation matrix! */
    void SetZero() { BIASABORT; }

  }; // class

} // namespace BIAS


#endif // __RMatrixBase_hh__