vpQuaternionVector.cpp

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2017 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:
 * Quaternion vector.
 *
 * Authors:
 * Filip Novotny
 *
 *****************************************************************************/


#include <visp3/core/vpQuaternionVector.h>
#include <visp3/core/vpMath.h>
#include <stdio.h>
#include <string.h>
#include <algorithm>


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

vpQuaternionVector::vpQuaternionVector()
  : vpRotationVector(4)
{}

vpQuaternionVector::vpQuaternionVector(const vpQuaternionVector &q)
  : vpRotationVector(q)
{}

vpQuaternionVector::vpQuaternionVector(const double x_, const double y_,
                                       const double z_,const double w_)
  : vpRotationVector(4) 
{
  set(x_, y_, z_, w_);
}

vpQuaternionVector::vpQuaternionVector(const vpColVector &q)
  : vpRotationVector(4)
{
  if (q.size() != 4) {
    throw(vpException(vpException::dimensionError, "Cannot construct a quaternion vector from a %d-dimension col vector", q.size()));
  }
  for (unsigned int i=0; i<4; i++)
    data[i] = q[i];
}

vpQuaternionVector::vpQuaternionVector(const vpRotationMatrix &R)  
  : vpRotationVector(4) 
{   
  buildFrom(R);
}

vpQuaternionVector::vpQuaternionVector(const vpThetaUVector& tu)
  : vpRotationVector (4)
{
  buildFrom(tu) ;
}


void vpQuaternionVector::set(const double qx, const double qy,
                             const double qz, const double qw)
{
  data[0]=qx;
  data[1]=qy;
  data[2]=qz;
  data[3]=qw;
}
vpQuaternionVector
vpQuaternionVector::buildFrom(const double qx, const double qy,
                              const double qz, const double qw)
{
  set(qx, qy, qz, qw);
  return *this;
}

vpQuaternionVector
vpQuaternionVector::buildFrom(const vpThetaUVector& tu)
{
  vpRotationMatrix R(tu) ;
  buildFrom(R) ;

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

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

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

vpQuaternionVector vpQuaternionVector::operator* (const vpQuaternionVector &rq) const {
  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/(const double l) const
{
  if(vpMath::nul(l, std::numeric_limits<double>::epsilon())) {
    throw vpException(vpException::fatalError, "Division by scalar l==0 !");
  }

  return vpQuaternionVector(x()/l,y()/l,z()/l,w()/l);
}
vpQuaternionVector &vpQuaternionVector::operator=(const vpColVector &q)
{
  if (q.size() != 4) {
    throw(vpException(vpException::dimensionError, "Cannot set a quaternion vector from a %d-dimension col vector", q.size()));
  }
  for (unsigned int i=0; i< 4; i++)
    data[i] = q[i];

  return *this;
}

vpQuaternionVector
vpQuaternionVector::buildFrom(const vpRotationMatrix &R)
{
  vpThetaUVector tu(R);
  vpColVector u;
  double theta;
  tu.extract(theta, u);

  theta *= 0.5;

  double sinTheta_2 = sin(theta);
  set( u[0] * sinTheta_2, u[1] * sinTheta_2, u[2] * sinTheta_2, cos(theta) );
  return *this;
}

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

vpQuaternionVector vpQuaternionVector::inverse() const {
  vpQuaternionVector q_inv;

  double mag_square = w()*w() + x()*x() + y()*y() + z()*z();
  if(!vpMath::nul(mag_square, std::numeric_limits<double>::epsilon())) {
    q_inv = this->conjugate() / mag_square;
  } else {
    std::cerr << "The current quaternion is null ! The inverse cannot be computed !" << std::endl;
  }

  return q_inv;
}

double vpQuaternionVector::magnitude() const {
  return sqrt( w()*w() + x()*x() + y()*y() + z()*z() );
}

void vpQuaternionVector::normalize() {
  double mag = magnitude();
  if(!vpMath::nul(mag, std::numeric_limits<double>::epsilon())) {
    set( x()/mag, y()/mag, z()/mag, w()/mag );
  }
}

double vpQuaternionVector::x() const {return data[0];}
double vpQuaternionVector::y() const {return data[1];}
double vpQuaternionVector::z() const {return data[2];}
double vpQuaternionVector::w() const {return data[3];}