visp_bridge: visp_bridge: vpQuaternionVector.cpp Source 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 
00043 #include "vpQuaternionVector.h"
00044 #include <visp/vpMath.h>
00045 #include <stdio.h>
00046 #include <string.h>
00047 #include <algorithm>
00048 
00049 
00050 // minimum value of sine
00051 const double vpQuaternionVector::minimum = 0.0001;
00052 
00057 vpQuaternionVector::vpQuaternionVector() : vpColVector(4) {  }
00058 
00060 vpQuaternionVector::vpQuaternionVector(const double x, const double y, 
00061                                        const double z,const double w) 
00062   : vpColVector(4)
00063 {
00064   set(x, y, z, w);
00065 }
00066 
00072 vpQuaternionVector::vpQuaternionVector(const vpRotationMatrix &R) :
00073     vpColVector(4)
00074 {       
00075   buildFrom(R);
00076 }
00081 vpQuaternionVector::vpQuaternionVector(const vpQuaternionVector &q) :
00082     vpColVector(4)
00083 {  
00084   for(unsigned int i=0;i<size();i++) (*this)[i]=q.r[i];   
00085 }
00086     
00094 void vpQuaternionVector::set(const double x, const double y, 
00095                              const double z,const double w) 
00096 {
00097   r[0]=x;
00098   r[1]=y;
00099   r[2]=z;
00100   r[3]=w;
00101 }
00102 
00103     
00111 vpQuaternionVector vpQuaternionVector::operator+( vpQuaternionVector &q)  
00112 {       
00113   return vpQuaternionVector(x()+q.x(), y()+q.y(), z()+q.z(), w()+q.w());
00114 }
00122 vpQuaternionVector vpQuaternionVector::operator-( vpQuaternionVector &q)  
00123 {
00124   return vpQuaternionVector(x()-q.x(), y()-q.y(), z()-q.z(), w()-q.w());
00125 }
00126 
00128 vpQuaternionVector vpQuaternionVector::operator-()  
00129 {
00130   return vpQuaternionVector(-x(), -y(), -z(), -w());
00131 }
00132 
00134 vpQuaternionVector vpQuaternionVector::operator*( double l) 
00135 {
00136   return vpQuaternionVector(l*x(),l*y(),l*z(),l*w());
00137 }
00138 
00140 vpQuaternionVector vpQuaternionVector::operator* ( vpQuaternionVector &rq) {    
00141   return vpQuaternionVector(w() * rq.x() + x() * rq.w() + y() * rq.z() - z() * rq.y(),
00142                             w() * rq.y() + y() * rq.w() + z() * rq.x() - x() * rq.z(),
00143                             w() * rq.z() + z() * rq.w() + x() * rq.y() - y() * rq.x(),
00144                             w() * rq.w() - x() * rq.x() - y() * rq.y() - z() * rq.z());
00145 }
00146 
00148 vpQuaternionVector &vpQuaternionVector::operator=( vpQuaternionVector &q)
00149 { 
00150   for(unsigned int i=0;i<size();i++) (*this)[i]=q.r[i];
00151   return *this;
00152 } 
00153 
00159 void vpQuaternionVector::buildFrom(const vpRotationMatrix &R)
00160 {
00161   double s,c,theta,sinc;
00162   double axis_x,axis_y,axis_z;
00163 
00164   s = (R[1][0]-R[0][1])*(R[1][0]-R[0][1])
00165       + (R[2][0]-R[0][2])*(R[2][0]-R[0][2])
00166       + (R[2][1]-R[1][2])*(R[2][1]-R[1][2]);
00167   s = sqrt(s)/2.0;
00168   c = (R[0][0]+R[1][1]+R[2][2]-1.0)/2.0;
00169   theta=atan2(s,c);  /* theta in [0, PI] since s > 0 */
00170   
00171   if ((s > minimum) || (c > 0.0)) { /* general case */  
00172     sinc = vpMath::sinc(s,theta);
00173       
00174     axis_x = (R[2][1]-R[1][2])/(2*sinc);
00175     axis_y = (R[0][2]-R[2][0])/(2*sinc);
00176     axis_z = (R[1][0]-R[0][1])/(2*sinc);
00177   } else { /* theta near PI */  
00178     axis_x = theta*(sqrt((R[0][0]-c)/(1-c)));
00179     if ((R[2][1]-R[1][2]) < 0) axis_x = -axis_x;
00180     axis_y = theta*(sqrt((R[1][1]-c)/(1-c)));
00181     if ((R[0][2]-R[2][0]) < 0) axis_y = -axis_y;
00182     axis_z = theta*(sqrt((R[2][2]-c)/(1-c)));
00183     if ((R[1][0]-R[0][1]) < 0) axis_z = -axis_z;
00184   }
00185         
00186   theta *= 0.5;
00187   double norm = sqrt(axis_x*axis_x+axis_y*axis_y+axis_z*axis_z);        
00188   if(fabs(norm)<minimum) norm = 1.;
00189   double sinTheta_2 = sin(theta);
00190   set((axis_x * sinTheta_2)/norm,
00191       (axis_y * sinTheta_2)/norm,
00192       (axis_z * sinTheta_2)/norm,
00193       cos(theta));
00194 
00195 }