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
00043 #include "vpQuaternionVector.h"
00044 #include <visp/vpMath.h>
00045 #include <stdio.h>
00046 #include <string.h>
00047 #include <algorithm>
00048
00049
00050
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);
00170
00171 if ((s > minimum) || (c > 0.0)) {
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 {
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 }