vpQuaternionVector.cpp

Go to the documentation of this file.

/****************************************************************************
 *
 * $Id: vpQuaternionVector.cpp 3514 2011-12-08 10:19:39Z fnovotny $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2011 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:
 * Quaternion vector.
 *
 * Authors:
 * Filip Novotny
 *
 *****************************************************************************/

#include <stdio.h>
#include <string.h>
#include <algorithm>

#include <ros/ros.h>
#include <visp/vpConfig.h>
#include <visp/vpMath.h>

#if VISP_VERSION_INT <= (2<<16 | 6<<8 | 1)

#include "visp_bridge/vpQuaternionVector.h"

// minimum value of sine
const double vpQuaternionVector::minimum = 0.0001;

vpQuaternionVector::vpQuaternionVector() : vpColVector(4) { }

vpQuaternionVector::vpQuaternionVector(const double x, const double y, 
                                       const double z,const double w) 
  : vpColVector(4)
{
  set(x, y, z, w);
}

vpQuaternionVector::vpQuaternionVector(const vpRotationMatrix &R) :
    vpColVector(4)
{       
  buildFrom(R);
}
vpQuaternionVector::vpQuaternionVector(const vpQuaternionVector &q) :
    vpColVector(4)
{  
  for(unsigned int i=0;i<size();i++) (*this)[i]=q.r[i];   
}
    
void vpQuaternionVector::set(const double x, const double y, 
                             const double z,const double w) 
{
  r[0]=x;
  r[1]=y;
  r[2]=z;
  r[3]=w;
}

    
vpQuaternionVector vpQuaternionVector::operator+( vpQuaternionVector &q)  
{       
  return vpQuaternionVector(x()+q.x(), y()+q.y(), z()+q.z(), w()+q.w());
}
vpQuaternionVector vpQuaternionVector::operator-( vpQuaternionVector &q)  
{
  return vpQuaternionVector(x()-q.x(), y()-q.y(), z()-q.z(), w()-q.w());
}

vpQuaternionVector vpQuaternionVector::operator-()  
{
  return vpQuaternionVector(-x(), -y(), -z(), -w());
}

vpQuaternionVector vpQuaternionVector::operator*( double l) 
{
  return vpQuaternionVector(l*x(),l*y(),l*z(),l*w());
}

vpQuaternionVector vpQuaternionVector::operator* ( vpQuaternionVector &rq) {    
  return vpQuaternionVector(w() * rq.x() + x() * rq.w() + y() * rq.z() - z() * rq.y(),
                            w() * rq.y() + y() * rq.w() + z() * rq.x() - x() * rq.z(),
                            w() * rq.z() + z() * rq.w() + x() * rq.y() - y() * rq.x(),
                            w() * rq.w() - x() * rq.x() - y() * rq.y() - z() * rq.z());
}

vpQuaternionVector &vpQuaternionVector::operator=( vpQuaternionVector &q)
{ 
  for(unsigned int i=0;i<size();i++) (*this)[i]=q.r[i];
  return *this;
} 

void vpQuaternionVector::buildFrom(const vpRotationMatrix &R)
{
  double s,c,theta,sinc;
  double axis_x,axis_y,axis_z;

  s = (R[1][0]-R[0][1])*(R[1][0]-R[0][1])
      + (R[2][0]-R[0][2])*(R[2][0]-R[0][2])
      + (R[2][1]-R[1][2])*(R[2][1]-R[1][2]);
  s = sqrt(s)/2.0;
  c = (R[0][0]+R[1][1]+R[2][2]-1.0)/2.0;
  theta=atan2(s,c);  /* theta in [0, PI] since s > 0 */
  
  if ((s > minimum) || (c > 0.0)) { /* general case */  
    sinc = vpMath::sinc(s,theta);
      
    axis_x = (R[2][1]-R[1][2])/(2*sinc);
    axis_y = (R[0][2]-R[2][0])/(2*sinc);
    axis_z = (R[1][0]-R[0][1])/(2*sinc);
  } else { /* theta near PI */  
    axis_x = theta*(sqrt((R[0][0]-c)/(1-c)));
    if ((R[2][1]-R[1][2]) < 0) axis_x = -axis_x;
    axis_y = theta*(sqrt((R[1][1]-c)/(1-c)));
    if ((R[0][2]-R[2][0]) < 0) axis_y = -axis_y;
    axis_z = theta*(sqrt((R[2][2]-c)/(1-c)));
    if ((R[1][0]-R[0][1]) < 0) axis_z = -axis_z;
  }
        
  theta *= 0.5;
  double norm = sqrt(axis_x*axis_x+axis_y*axis_y+axis_z*axis_z);        
  if(fabs(norm)<minimum) norm = 1.;
  double sinTheta_2 = sin(theta);
  set((axis_x * sinTheta_2)/norm,
      (axis_y * sinTheta_2)/norm,
      (axis_z * sinTheta_2)/norm,
      cos(theta));

}

#endif