vpForceTwistMatrix.cpp

/****************************************************************************
 *
 * $Id: vpForceTwistMatrix.cpp 4649 2014-02-07 14:57:11Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2014 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://www.irisa.fr/lagadic/visp/visp.html for more information.
 * 
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * 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 <visp/vpForceTwistMatrix.h>

// Exception
#include <visp/vpException.h>
#include <visp/vpMatrixException.h>

// Debug trace
#include <visp/vpDebug.h>


vpForceTwistMatrix &
vpForceTwistMatrix::operator=(const vpForceTwistMatrix &M)
{
  init() ;

  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::init()
{
  unsigned int i,j ;

  try {
    resize(6,6) ;
  }
  catch(vpException me) {
    vpERROR_TRACE("Error caught") ;
    throw ;
  }

  for (i=0 ; i < 6 ; i++) {
    for (j=0 ; j < 6; j++) {
      if (i==j)
    (*this)[i][j] = 1.0 ;
      else
    (*this)[i][j] = 0.0;
    }
  }
}

vpForceTwistMatrix::vpForceTwistMatrix() : vpMatrix()
{
  init() ;
}

vpForceTwistMatrix::vpForceTwistMatrix(const vpForceTwistMatrix &F) : vpMatrix()
{
  init() ;
  *this = F ;
}

vpForceTwistMatrix::vpForceTwistMatrix(const vpHomogeneousMatrix &M) : vpMatrix()
{
  init() ;
  buildFrom(M);
}

vpForceTwistMatrix::vpForceTwistMatrix(const vpTranslationVector &tv,
                                       const vpThetaUVector &thetau) : vpMatrix()
{
  init() ;
  buildFrom(tv, thetau) ;
}

vpForceTwistMatrix::vpForceTwistMatrix(const vpTranslationVector &tv,
                       const vpRotationMatrix &R)
{
  init() ;
  buildFrom(tv,R) ;
}

vpForceTwistMatrix::vpForceTwistMatrix(const double tx,
                       const double ty,
                       const double tz,
                       const double tux,
                       const double tuy,
                       const double tuz) : vpMatrix()
{
  init() ;
  vpTranslationVector T(tx,ty,tz) ;
  vpThetaUVector tu(tux,tuy,tuz) ;
  buildFrom(T,tu) ;  
}

void
vpForceTwistMatrix::setIdentity()
{
  init() ;
}


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())
  {
    vpERROR_TRACE("vpForceTwistMatrix mismatch in vpForceTwistMatrix/vpMatrix multiply") ;
    throw(vpMatrixException::incorrectMatrixSizeError) ;
  }

  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())
  {
    vpERROR_TRACE("vpForceTwistMatrix mismatch in vpForceTwistMatrix/vector multiply") ;
    throw(vpMatrixException::incorrectMatrixSizeError) ;
  }

  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 &tv,
                              const vpRotationMatrix &R)
{
  unsigned int i, j;
  vpMatrix skewaR = tv.skew(tv)*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) ;
}

/*
 * Local variables:
 * c-basic-offset: 2
 * End:
 */