Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
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
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);
00174
00175 if ((s > minimum) || (c > 0.0)) {
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 {
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