vpVelocityTwistMatrix.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:
 * Velocity twist transformation matrix.
 *
 * Authors:
 * Fabien Spindler
 *
 *****************************************************************************/

#include <sstream>
#include <assert.h>

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


vpVelocityTwistMatrix &
vpVelocityTwistMatrix::operator=(const vpVelocityTwistMatrix &V)
{
  for (int i=0; i<6; i++)
  {
    for (int j=0; j<6; j++)
    {
      rowPtrs[i][j] = V.rowPtrs[i][j];
    }
  }

  return *this;
}


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

vpVelocityTwistMatrix::vpVelocityTwistMatrix()
  : vpArray2D<double>(6, 6)
{
  eye() ;
}

vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpVelocityTwistMatrix &V)
  : vpArray2D<double>(6, 6)
{
  *this = V;
}

vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpHomogeneousMatrix &M)
  : vpArray2D<double>(6, 6)
{
  buildFrom(M);
}

vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpTranslationVector &t,
                                             const vpThetaUVector &thetau)
  : vpArray2D<double>(6, 6)
{
  buildFrom(t, thetau) ;
}

vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpTranslationVector &t,
                                             const vpRotationMatrix &R)
  : vpArray2D<double>(6, 6)
{
  buildFrom(t,R) ;
}

vpVelocityTwistMatrix::vpVelocityTwistMatrix(const double tx,
                         const double ty,
                         const double tz,
                         const double tux,
                         const double tuy,
               const double tuz)
  : vpArray2D<double>(6, 6)
{
  vpTranslationVector T(tx,ty,tz) ;
  vpThetaUVector tu(tux,tuy,tuz) ;
  buildFrom(T,tu) ;  
}

void
vpVelocityTwistMatrix::setIdentity()
{
  eye() ;
}


vpVelocityTwistMatrix
vpVelocityTwistMatrix::operator*(const vpVelocityTwistMatrix &V) const
{
  vpVelocityTwistMatrix p ;

  for (unsigned int i=0;i<6;i++)
    for (unsigned int j=0;j<6;j++)
    {
      double s =0 ;
      for (int k=0;k<6;k++)
        s +=rowPtrs[i][k] * V.rowPtrs[k][j];
      p[i][j] = s ;
    }
  return p;
}

vpMatrix
vpVelocityTwistMatrix::operator*(const vpMatrix &M) const
{
  if (6 != M.getRows()) {
    throw(vpException(vpException::dimensionError,
                      "Cannot multiply a (6x6) velocity twist matrix by a (%dx%d) matrix",
                      M.getRows(), M.getCols()));
  }

  vpMatrix p(6, M.getCols()) ;
  for (unsigned int i=0;i<6;i++)
    for (unsigned int j=0;j<M.getCols();j++)
      {
    double s =0 ;
    for (unsigned int k=0;k<6;k++)
      s += rowPtrs[i][k] * M[k][j];
    p[i][j] = s ;
      }
  return p;
}

vpColVector
vpVelocityTwistMatrix::operator*(const vpColVector &v) const
{
  vpColVector c(6);

  if (6 != v.getRows())
  {
    throw(vpException(vpException::dimensionError,
                      "Cannot multiply a (6x6) velocity twist matrix by a (%d) column vector",
                      v.getRows()));
  }

  c = 0.0;

  for (unsigned int i=0;i<6;i++) {
    for (unsigned int j=0;j<6;j++) {
      {
        c[i]+=rowPtrs[i][j] * v[j];
      }
    }
  }

  return c ;
}


vpVelocityTwistMatrix
vpVelocityTwistMatrix::buildFrom(const vpTranslationVector &t,
                                 const vpRotationMatrix &R)
{
  unsigned int i, j;
  vpMatrix skewaR = t.skew(t)*R ;

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

    }
  return (*this) ;
}

vpVelocityTwistMatrix
vpVelocityTwistMatrix::buildFrom(const vpTranslationVector &t,
                                 const vpThetaUVector &thetau)
{
  vpRotationMatrix R ;
  R.buildFrom(thetau) ;
  buildFrom(t,R) ;
  return (*this) ;
}


vpVelocityTwistMatrix
vpVelocityTwistMatrix::buildFrom(const vpHomogeneousMatrix &M)
{
  vpTranslationVector tv ;
  vpRotationMatrix R ;
  M.extract(R) ;
  M.extract(tv) ;

  buildFrom(tv, R) ;
  return (*this) ;
}


vpVelocityTwistMatrix
vpVelocityTwistMatrix::inverse() const
{
  vpVelocityTwistMatrix Wi;
  vpRotationMatrix R;extract(R);
  vpTranslationVector T;extract(T);
  vpTranslationVector RtT ; RtT = -(R.t()*T) ;

  Wi.buildFrom(RtT,R.t());

  return Wi ;
}


void
vpVelocityTwistMatrix::inverse(vpVelocityTwistMatrix &V) const
{
  V = inverse();
}

void
vpVelocityTwistMatrix::extract( vpRotationMatrix &R) const
{
    for (unsigned int i=0 ; i < 3 ; i++)
        for (unsigned int j=0 ; j < 3; j++)
          R[i][j] = (*this)[i][j];
}

void
vpVelocityTwistMatrix::extract(vpTranslationVector &tv) const
{
    vpRotationMatrix R;extract(R);
    vpMatrix skTR(3,3);
    for (unsigned int i=0 ; i < 3 ; i++)
      for (unsigned int j=0 ; j < 3; j++)
        skTR[i][j] = (*this)[i][j+3];

    vpMatrix skT = skTR*R.t();
  tv[0] = skT[2][1];
  tv[1] = skT[0][2];
  tv[2] = skT[1][0];
}

int
vpVelocityTwistMatrix::print(std::ostream& s, unsigned int length, char const* intro) const
{
  typedef std::string::size_type size_type;

  unsigned int m = getRows();
  unsigned int n = getCols();

  std::vector<std::string> values(m*n);
  std::ostringstream oss;
  std::ostringstream ossFixed;
  std::ios_base::fmtflags original_flags = oss.flags();

  // ossFixed <<std::fixed;
  ossFixed.setf ( std::ios::fixed, std::ios::floatfield );

  size_type maxBefore=0;  // the length of the integral part
  size_type maxAfter=0;   // number of decimals plus
  // one place for the decimal point
  for (unsigned int i=0;i<m;++i) {
    for (unsigned int j=0;j<n;++j){
      oss.str("");
      oss << (*this)[i][j];
      if (oss.str().find("e")!=std::string::npos){
        ossFixed.str("");
        ossFixed << (*this)[i][j];
        oss.str(ossFixed.str());
      }

      values[i*n+j]=oss.str();
      size_type thislen=values[i*n+j].size();
      size_type p=values[i*n+j].find('.');

      if (p==std::string::npos){
        maxBefore=vpMath::maximum(maxBefore, thislen);
        // maxAfter remains the same
      } else{
        maxBefore=vpMath::maximum(maxBefore, p);
        maxAfter=vpMath::maximum(maxAfter, thislen-p-1);
      }
    }
  }

  size_type totalLength=length;
  // increase totalLength according to maxBefore
  totalLength=vpMath::maximum(totalLength,maxBefore);
  // decrease maxAfter according to totalLength
  maxAfter=std::min(maxAfter, totalLength-maxBefore);
  if (maxAfter==1) maxAfter=0;

  // the following line is useful for debugging
  //std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";

  if (intro) s <<intro;
  s <<"["<<m<<","<<n<<"]=\n";

  for (unsigned int i=0;i<m;i++) {
    s <<"  ";
    for (unsigned int j=0;j<n;j++){
      size_type p=values[i*n+j].find('.');
      s.setf(std::ios::right, std::ios::adjustfield);
      s.width((std::streamsize)maxBefore);
      s <<values[i*n+j].substr(0,p).c_str();

      if (maxAfter>0){
        s.setf(std::ios::left, std::ios::adjustfield);
        if (p!=std::string::npos){
          s.width((std::streamsize)maxAfter);
          s <<values[i*n+j].substr(p,maxAfter).c_str();
        } else{
          assert(maxAfter>1);
          s.width((std::streamsize)maxAfter);
          s <<".0";
        }
      }

      s <<' ';
    }
    s <<std::endl;
  }

  s.flags(original_flags); // restore s to standard state

  return (int)(maxBefore+maxAfter);
}