vpVelocityTwistMatrix.cpp

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

#include <visp/vpVelocityTwistMatrix.h>

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

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


vpVelocityTwistMatrix &
vpVelocityTwistMatrix::operator=(const vpVelocityTwistMatrix &V)
{
  init() ;

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

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

vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpVelocityTwistMatrix &V) : vpMatrix()
{
  init() ;
  *this = V;
}

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

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

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

vpVelocityTwistMatrix::vpVelocityTwistMatrix(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
vpVelocityTwistMatrix::setIdentity()
{
  init() ;
}


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())
    {
      vpERROR_TRACE("vpVelocityTwistMatrix mismatch in vpVelocityTwistMatrix/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
vpVelocityTwistMatrix::operator*(const vpColVector &v) const
{
  vpColVector c(6);

  if (6 != v.getRows())
  {
    vpERROR_TRACE("vpVelocityTwistMatrix mismatch in vpVelocityTwistMatrix/vector multiply") ;
    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
                            "Mismatch in vpVelocityTwistMatrix/vector multiply")) ;
  }

  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 &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][j+3] = skewaR[i][j] ;

    }
  return (*this) ;
}

vpVelocityTwistMatrix
vpVelocityTwistMatrix::buildFrom(const vpTranslationVector &tv,
                 const vpThetaUVector &thetau)
{
  vpRotationMatrix R ;
  R.buildFrom(thetau) ;
  buildFrom(tv,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 &Wi) const
{
    Wi = 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];
}

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