vpRotationMatrix.cpp

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Rotation matrix.
 *
 * Authors:
 * Eric Marchand
 *
 *****************************************************************************/

#include <visp3/core/vpMath.h>
#include <visp3/core/vpMatrix.h>

// Rotation classes
#include <visp3/core/vpRotationMatrix.h>


// Exception
#include <visp3/core/vpException.h>

// Debug trace
#include <visp3/core/vpDebug.h>
#include <math.h>
const double vpRotationMatrix::threshold = 1e-6;


void
vpRotationMatrix::setIdentity()
{
  eye();
}
void
vpRotationMatrix::eye()
{
  for (unsigned int i=0 ; i < 3 ; i++) {
    for (unsigned int j=0 ; j < 3; j++) {
      if (i==j)
        (*this)[i][j] = 1.0;
      else
        (*this)[i][j] = 0.0;
    }
  }
}

vpRotationMatrix &
vpRotationMatrix::operator=(const vpRotationMatrix &R)
{
  for (unsigned int i=0; i<3; i++) {
    for (unsigned int j=0; j<3; j++) {
      rowPtrs[i][j] = R.rowPtrs[i][j];
    }
  }

  return *this;
}

vpRotationMatrix &
vpRotationMatrix::operator=(const vpMatrix &M)
{
  if ((M.getCols() !=3) &&(M.getRows() !=3)) {
    throw(vpException(vpException::dimensionError,
                      "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix",
                      M.getRows(), M.getCols()));
  }

  for (unsigned int i=0; i<3; i++) {
    for (unsigned int j=0; j<3; j++) {
      (*this)[i][j] = M[i][j];
    }
  }

  if (isARotationMatrix() == false) {
    throw(vpException(vpException::fatalError,
                      "Cannot set a rotation matrix from a matrix that is not a rotation matrix"));
  }

  return *this;
}

vpRotationMatrix
vpRotationMatrix::operator*(const vpRotationMatrix &R) const
{
  vpRotationMatrix p ;

  for (unsigned int i=0;i<3;i++) {
    for (unsigned int j=0;j<3;j++) {
      double s =0 ;
      for (unsigned int k=0;k<3;k++)
        s +=rowPtrs[i][k] * R.rowPtrs[k][j];
      p[i][j] = s ;
    }
  }
  return p;
}
vpMatrix
vpRotationMatrix::operator*(const vpMatrix &M) const
{
  if (M.getRows() != 3 || M.getCols() != 3) {
    throw(vpException(vpException::dimensionError,
                      "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix",
                      M.getRows(), M.getCols()));
  }
  vpMatrix p(3, 3) ;

  for (unsigned int i=0;i<3;i++) {
    for (unsigned int j=0;j<3;j++) {
      double s =0 ;
      for (unsigned int k=0;k<3;k++)
        s +=(*this)[i][k] * M[k][j];
      p[i][j] = s ;
    }
  }
  return p;
}

vpColVector
vpRotationMatrix::operator*(const vpColVector &v) const
{
  if (v.getRows() != 3) {
    throw (vpException(vpException::dimensionError,
                       "Cannot multiply a (3x3) rotation matrix by a %d dimension column vector",
                       v.getRows()));
  }
  vpColVector v_out(3);

  for (unsigned int j=0;j<colNum;j++) {
    double vj = v[j] ; // optimization em 5/12/2006
    for (unsigned int i=0;i<rowNum;i++) {
      v_out[i] += rowPtrs[i][j] * vj;
    }
  }

  return v_out;
}


vpTranslationVector
vpRotationMatrix::operator*(const vpTranslationVector &tv) const
{
  vpTranslationVector p ;

  for (unsigned int j=0;j<3;j++)
    p[j] = 0;

  for (unsigned int j=0;j<3;j++) {
    for (unsigned int i=0;i<3;i++) {
      p[i] += rowPtrs[i][j] * tv[j];
    }
  }

  return p;
}

vpRotationMatrix vpRotationMatrix::operator*(double x) const
{
  vpRotationMatrix R;

  for (unsigned int i=0;i<rowNum;i++)
    for(unsigned int j=0;j<colNum;j++)
      R[i][j]= rowPtrs[i][j]*x;

  return R;
}

vpRotationMatrix & vpRotationMatrix::operator*=(double x)
{
  for (unsigned int i=0;i<rowNum;i++)
    for(unsigned int j=0;j<colNum;j++)
      rowPtrs[i][j]*=x;

  return *this;
}

/*********************************************************************/

bool
vpRotationMatrix::isARotationMatrix() const
{
  unsigned int i,j ;
  bool isRotation = true ;

  // test R^TR = Id ;
  vpRotationMatrix RtR = (*this).t()*(*this) ;
  for (i=0 ; i < 3 ; i++) {
    for (j=0 ; j < 3 ; j++) {
      if (i==j) {
        if (fabs(RtR[i][j]-1) > threshold)  isRotation = false ;
      }
      else {
        if (fabs(RtR[i][j]) > threshold)  isRotation = false ;
      }
    }
  }
  // test if it is a basis
  // test || Ci || = 1
  for (i=0 ; i < 3 ; i++) {
    if ((sqrt(vpMath::sqr(RtR[0][i]) +
              vpMath::sqr(RtR[1][i]) +
              vpMath::sqr(RtR[2][i])) - 1) > threshold)  isRotation = false ;
  }

  // test || Ri || = 1
  for (i=0 ; i < 3 ; i++) {
    if ((sqrt(vpMath::sqr(RtR[i][0]) +
              vpMath::sqr(RtR[i][1]) +
              vpMath::sqr(RtR[i][2])) - 1) > threshold)  isRotation = false ;
  }

  //  test if the basis is orthogonal
  return isRotation ;
}


vpRotationMatrix::vpRotationMatrix() : vpArray2D<double>(3,3)
{
  eye();
}


vpRotationMatrix::vpRotationMatrix(const vpRotationMatrix &M) : vpArray2D<double>(3,3)
{
  (*this) = M ;
}
vpRotationMatrix::vpRotationMatrix(const vpHomogeneousMatrix &M) : vpArray2D<double>(3,3)
{
  buildFrom(M);
}

vpRotationMatrix::vpRotationMatrix(const vpThetaUVector &tu) : vpArray2D<double>(3,3)
{
  buildFrom(tu) ;
}

vpRotationMatrix::vpRotationMatrix(const vpPoseVector &p) : vpArray2D<double>(3,3)
{
  buildFrom(p) ;
}

vpRotationMatrix::vpRotationMatrix(const vpRzyzVector &euler) : vpArray2D<double>(3,3)
{
  buildFrom(euler) ;
}

vpRotationMatrix::vpRotationMatrix(const vpRxyzVector &Rxyz) : vpArray2D<double>(3,3)
{
  buildFrom(Rxyz) ;
}

vpRotationMatrix::vpRotationMatrix(const vpRzyxVector &Rzyx) : vpArray2D<double>(3,3)
{
  buildFrom(Rzyx) ;
}

vpRotationMatrix::vpRotationMatrix(const double tux, const double tuy, const double tuz) : vpArray2D<double>(3,3)
{
  buildFrom(tux, tuy, tuz) ;
}

vpRotationMatrix::vpRotationMatrix(const vpQuaternionVector& q) : vpArray2D<double>(3,3)
{
  buildFrom(q);
}

vpRotationMatrix
vpRotationMatrix::t() const
{
  vpRotationMatrix Rt ;

  unsigned int i,j;
  for (i=0;i<3;i++)
    for (j=0;j<3;j++)
      Rt[j][i] = (*this)[i][j];

  return Rt;
}

vpRotationMatrix vpRotationMatrix::inverse() const
{
  vpRotationMatrix Ri = (*this).t() ;

  return Ri ;
}

void
vpRotationMatrix::inverse(vpRotationMatrix &R) const
{
  R = inverse();
}

void
vpRotationMatrix::printVector()
{
  vpThetaUVector tu(*this) ;

  for (unsigned int i=0; i<3; i++)
    std::cout << tu[i] << "  " ;

  std::cout << std::endl ;
}


vpRotationMatrix
vpRotationMatrix::buildFrom(const vpThetaUVector &v)
{
  unsigned int i,j;
  double theta, si, co, sinc, mcosc;
  vpRotationMatrix R;

  theta = sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
  si = sin(theta);
  co = cos(theta);
  sinc = vpMath::sinc(si,theta);
  mcosc = vpMath::mcosc(co,theta);

  R[0][0] = co + mcosc*v[0]*v[0];
  R[0][1] = -sinc*v[2] + mcosc*v[0]*v[1];
  R[0][2] = sinc*v[1] + mcosc*v[0]*v[2];
  R[1][0] = sinc*v[2] + mcosc*v[1]*v[0];
  R[1][1] = co + mcosc*v[1]*v[1];
  R[1][2] = -sinc*v[0] + mcosc*v[1]*v[2];
  R[2][0] = -sinc*v[1] + mcosc*v[2]*v[0];
  R[2][1] = sinc*v[0] + mcosc*v[2]*v[1];
  R[2][2] = co + mcosc*v[2]*v[2];

  for (i=0;i<3;i++) for (j=0;j<3;j++) (*this)[i][j] = R[i][j];

  return *this ;
}

vpRotationMatrix
vpRotationMatrix::buildFrom(const vpHomogeneousMatrix &M)
{
  for (unsigned int i=0 ; i < 3 ; i++)
    for (unsigned int j=0 ; j < 3; j++)
      (*this)[i][j] = M[i][j] ;

  return *this ;
}

vpRotationMatrix
vpRotationMatrix::buildFrom(const vpPoseVector &p)
{
  vpThetaUVector tu(p);
  return buildFrom(tu);
}

vpRotationMatrix
vpRotationMatrix::buildFrom(const vpRzyzVector &v)
{
  double c0,c1,c2,s0,s1,s2;

  c0 = cos(v[0]);
  c1 = cos(v[1]);
  c2 = cos(v[2]);
  s0 = sin(v[0]);
  s1 = sin(v[1]);
  s2 = sin(v[2]);

  (*this)[0][0] = c0*c1*c2 - s0*s2;
  (*this)[0][1] = -c0*c1*s2 - s0*c2;
  (*this)[0][2] = c0*s1;
  (*this)[1][0] = s0*c1*c2+c0*s2 ;
  (*this)[1][1] = -s0*c1*s2 + c0*c2 ;
  (*this)[1][2] = s0*s1;
  (*this)[2][0] = -s1*c2;
  (*this)[2][1] = s1*s2;
  (*this)[2][2] = c1;

  return (*this) ;
}


vpRotationMatrix
vpRotationMatrix::buildFrom(const vpRxyzVector &v)
{
  double c0,c1,c2,s0,s1,s2;

  c0 = cos(v[0]);
  c1 = cos(v[1]);
  c2 = cos(v[2]);
  s0 = sin(v[0]);
  s1 = sin(v[1]);
  s2 = sin(v[2]);

  (*this)[0][0] = c1*c2;
  (*this)[0][1] = -c1*s2;
  (*this)[0][2] = s1;
  (*this)[1][0] = c0*s2+s0*s1*c2;
  (*this)[1][1] = c0*c2-s0*s1*s2;
  (*this)[1][2] = -s0*c1;
  (*this)[2][0] = -c0*s1*c2+s0*s2;
  (*this)[2][1] = c0*s1*s2+c2*s0;
  (*this)[2][2] = c0*c1;

  return (*this) ;
}



vpRotationMatrix
vpRotationMatrix::buildFrom(const vpRzyxVector &v)
{
  double c0,c1,c2,s0,s1,s2;

  c0 = cos(v[0]);
  c1 = cos(v[1]);
  c2 = cos(v[2]);
  s0 = sin(v[0]);
  s1 = sin(v[1]);
  s2 = sin(v[2]);

  (*this)[0][0] = c0*c1 ;
  (*this)[0][1] = c0*s1*s2 - s0*c2 ;
  (*this)[0][2] = c0*s1*c2 + s0*s2 ;

  (*this)[1][0] = s0*c1 ;
  (*this)[1][1] = s0*s1*s2 + c0*c2 ;
  (*this)[1][2] = s0*s1*c2 - c0*s2 ;

  (*this)[2][0] = -s1 ;
  (*this)[2][1] = c1*s2 ;
  (*this)[2][2] = c1*c2 ;

  return (*this) ;
}



vpRotationMatrix
vpRotationMatrix::buildFrom(const double tux,
                            const double tuy,
                            const double tuz)
{
  vpThetaUVector tu(tux, tuy, tuz) ;
  buildFrom(tu) ;
  return *this ;
}


vpRotationMatrix
vpRotationMatrix::buildFrom(const vpQuaternionVector& q){
  double a = q.w();
  double b = q.x();
  double c = q.y();
  double d = q.z();
  (*this)[0][0] = a*a+b*b-c*c-d*d;
  (*this)[0][1] = 2*b*c-2*a*d;
  (*this)[0][2] = 2*a*c+2*b*d;

  (*this)[1][0] = 2*a*d+2*b*c;
  (*this)[1][1] = a*a-b*b+c*c-d*d;
  (*this)[1][2] = 2*c*d-2*a*b;

  (*this)[2][0] = 2*b*d-2*a*c;
  (*this)[2][1] = 2*a*b+2*c*d;
  (*this)[2][2] = a*a-b*b-c*c+d*d;
  return *this;
}

vpRotationMatrix operator*(const double &x,const vpRotationMatrix &R)
{
  vpRotationMatrix C;

  unsigned int Rrow = R.getRows() ;
  unsigned int Rcol = R.getCols() ;

  for (unsigned int i=0;i<Rrow;i++)
    for(unsigned int j=0;j<Rcol;j++)
      C[i][j]= R[i][j]*x;

  return C ;
}

vpThetaUVector vpRotationMatrix::getThetaUVector()
{
  vpThetaUVector tu;
  tu.buildFrom(*this);
  return tu;
}

vpColVector
vpRotationMatrix::getCol(const unsigned int j) const
{
  if (j >= getCols())
    throw(vpException(vpException::dimensionError,
                      "Unable to extract a column vector from the homogeneous matrix"));
  unsigned int nb_rows = getRows();
  vpColVector c(nb_rows);
  for (unsigned int i=0 ; i < nb_rows ; i++)
    c[i] = (*this)[i][j];
  return c;
}