vpForceTwistMatrix.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:
 * Twist transformation matrix that allows to transform forces from one 
 * frame to an other.
 *
 * Authors:
 * Fabien Spindler
 *
 *****************************************************************************/

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

#include <visp3/core/vpForceTwistMatrix.h>
#include <visp3/core/vpException.h>
#include <visp3/core/vpDebug.h>


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

  return *this;
}


void
vpForceTwistMatrix::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;
    }
  }
}

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

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

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

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

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

vpForceTwistMatrix::vpForceTwistMatrix(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
vpForceTwistMatrix::setIdentity()
{
  eye() ;
}


vpForceTwistMatrix
vpForceTwistMatrix::operator*(const vpForceTwistMatrix &F) const
{
  vpForceTwistMatrix Fout ;

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

vpMatrix
vpForceTwistMatrix::operator*(const vpMatrix &M) const
{

  if (6 != M.getRows()) {
    throw(vpException(vpException::dimensionError,
                      "Cannot multiply (6x6) force/torque 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
vpForceTwistMatrix::operator*(const vpColVector &H) const
{
  vpColVector Hout(6);

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

  Hout = 0.0;

  for (unsigned int i=0;i<6;i++) {
    for (unsigned int j=0;j<6;j++) {
      Hout[i]+=rowPtrs[i][j] * H[j];
    }
  }
  
  return Hout ;
}


vpForceTwistMatrix
vpForceTwistMatrix::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+3][j] = skewaR[i][j] ;
    }
  }
  return (*this) ;
}

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


vpForceTwistMatrix
vpForceTwistMatrix::buildFrom(const vpHomogeneousMatrix &M)
{
  vpTranslationVector tv ;
  vpRotationMatrix R ;
  M.extract(R) ;
  M.extract(tv) ;
  
  buildFrom(tv, R) ;
  return (*this) ;
}

int
vpForceTwistMatrix::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);
}