vpQuaternionVector.cpp
Go to the documentation of this file.
00001 /****************************************************************************
00002  *
00003  * $Id: vpQuaternionVector.cpp 3514 2011-12-08 10:19:39Z fnovotny $
00004  *
00005  * This file is part of the ViSP software.
00006  * Copyright (C) 2005 - 2011 by INRIA. All rights reserved.
00007  * 
00008  * This software is free software; you can redistribute it and/or
00009  * modify it under the terms of the GNU General Public License
00010  * ("GPL") version 2 as published by the Free Software Foundation.
00011  * See the file LICENSE.txt at the root directory of this source
00012  * distribution for additional information about the GNU GPL.
00013  *
00014  * For using ViSP with software that can not be combined with the GNU
00015  * GPL, please contact INRIA about acquiring a ViSP Professional 
00016  * Edition License.
00017  *
00018  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
00019  * 
00020  * This software was developed at:
00021  * INRIA Rennes - Bretagne Atlantique
00022  * Campus Universitaire de Beaulieu
00023  * 35042 Rennes Cedex
00024  * France
00025  * http://www.irisa.fr/lagadic
00026  *
00027  * If you have questions regarding the use of this file, please contact
00028  * INRIA at visp@inria.fr
00029  * 
00030  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
00031  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
00032  *
00033  *
00034  * Description:
00035  * Quaternion vector.
00036  *
00037  * Authors:
00038  * Filip Novotny
00039  *
00040  *****************************************************************************/
00041 
00042 #include <stdio.h>
00043 #include <string.h>
00044 #include <algorithm>
00045 
00046 #include <ros/ros.h>
00047 #include <visp/vpConfig.h>
00048 #include <visp/vpMath.h>
00049 
00050 #if VISP_VERSION_INT <= (2<<16 | 6<<8 | 1)
00051 
00052 #include "visp_bridge/vpQuaternionVector.h"
00053 
00054 // minimum value of sine
00055 const double vpQuaternionVector::minimum = 0.0001;
00056 
00061 vpQuaternionVector::vpQuaternionVector() : vpColVector(4) { }
00062 
00064 vpQuaternionVector::vpQuaternionVector(const double x, const double y, 
00065                                        const double z,const double w) 
00066   : vpColVector(4)
00067 {
00068   set(x, y, z, w);
00069 }
00070 
00076 vpQuaternionVector::vpQuaternionVector(const vpRotationMatrix &R) :
00077     vpColVector(4)
00078 {       
00079   buildFrom(R);
00080 }
00085 vpQuaternionVector::vpQuaternionVector(const vpQuaternionVector &q) :
00086     vpColVector(4)
00087 {  
00088   for(unsigned int i=0;i<size();i++) (*this)[i]=q.r[i];   
00089 }
00090     
00098 void vpQuaternionVector::set(const double x, const double y, 
00099                              const double z,const double w) 
00100 {
00101   r[0]=x;
00102   r[1]=y;
00103   r[2]=z;
00104   r[3]=w;
00105 }
00106 
00107     
00115 vpQuaternionVector vpQuaternionVector::operator+( vpQuaternionVector &q)  
00116 {       
00117   return vpQuaternionVector(x()+q.x(), y()+q.y(), z()+q.z(), w()+q.w());
00118 }
00126 vpQuaternionVector vpQuaternionVector::operator-( vpQuaternionVector &q)  
00127 {
00128   return vpQuaternionVector(x()-q.x(), y()-q.y(), z()-q.z(), w()-q.w());
00129 }
00130 
00132 vpQuaternionVector vpQuaternionVector::operator-()  
00133 {
00134   return vpQuaternionVector(-x(), -y(), -z(), -w());
00135 }
00136 
00138 vpQuaternionVector vpQuaternionVector::operator*( double l) 
00139 {
00140   return vpQuaternionVector(l*x(),l*y(),l*z(),l*w());
00141 }
00142 
00144 vpQuaternionVector vpQuaternionVector::operator* ( vpQuaternionVector &rq) {    
00145   return vpQuaternionVector(w() * rq.x() + x() * rq.w() + y() * rq.z() - z() * rq.y(),
00146                             w() * rq.y() + y() * rq.w() + z() * rq.x() - x() * rq.z(),
00147                             w() * rq.z() + z() * rq.w() + x() * rq.y() - y() * rq.x(),
00148                             w() * rq.w() - x() * rq.x() - y() * rq.y() - z() * rq.z());
00149 }
00150 
00152 vpQuaternionVector &vpQuaternionVector::operator=( vpQuaternionVector &q)
00153 { 
00154   for(unsigned int i=0;i<size();i++) (*this)[i]=q.r[i];
00155   return *this;
00156 } 
00157 
00163 void vpQuaternionVector::buildFrom(const vpRotationMatrix &R)
00164 {
00165   double s,c,theta,sinc;
00166   double axis_x,axis_y,axis_z;
00167 
00168   s = (R[1][0]-R[0][1])*(R[1][0]-R[0][1])
00169       + (R[2][0]-R[0][2])*(R[2][0]-R[0][2])
00170       + (R[2][1]-R[1][2])*(R[2][1]-R[1][2]);
00171   s = sqrt(s)/2.0;
00172   c = (R[0][0]+R[1][1]+R[2][2]-1.0)/2.0;
00173   theta=atan2(s,c);  /* theta in [0, PI] since s > 0 */
00174   
00175   if ((s > minimum) || (c > 0.0)) { /* general case */  
00176     sinc = vpMath::sinc(s,theta);
00177       
00178     axis_x = (R[2][1]-R[1][2])/(2*sinc);
00179     axis_y = (R[0][2]-R[2][0])/(2*sinc);
00180     axis_z = (R[1][0]-R[0][1])/(2*sinc);
00181   } else { /* theta near PI */  
00182     axis_x = theta*(sqrt((R[0][0]-c)/(1-c)));
00183     if ((R[2][1]-R[1][2]) < 0) axis_x = -axis_x;
00184     axis_y = theta*(sqrt((R[1][1]-c)/(1-c)));
00185     if ((R[0][2]-R[2][0]) < 0) axis_y = -axis_y;
00186     axis_z = theta*(sqrt((R[2][2]-c)/(1-c)));
00187     if ((R[1][0]-R[0][1]) < 0) axis_z = -axis_z;
00188   }
00189         
00190   theta *= 0.5;
00191   double norm = sqrt(axis_x*axis_x+axis_y*axis_y+axis_z*axis_z);        
00192   if(fabs(norm)<minimum) norm = 1.;
00193   double sinTheta_2 = sin(theta);
00194   set((axis_x * sinTheta_2)/norm,
00195       (axis_y * sinTheta_2)/norm,
00196       (axis_z * sinTheta_2)/norm,
00197       cos(theta));
00198 
00199 }
00200 
00201 #endif


visp_bridge
Author(s): Filip Novotny
autogenerated on Fri Aug 28 2015 13:36:28