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 #ifndef CQuaternion_H
00030 #define CQuaternion_H
00031
00032 #include <mrpt/utils/utils_defs.h>
00033 #include <mrpt/math/CMatrixTemplateNumeric.h>
00034 #include <mrpt/math/CVectorTemplate.h>
00035 #include <cmath>
00036
00037 namespace mrpt
00038 {
00039 namespace math
00040 {
00041
00042
00043
00044
00045
00046
00047 template <class T>
00048 class CQuaternion:public CVectorTemplate<T>
00049 {
00050 public:
00051
00052
00053
00054
00055
00056
00057 CQuaternion() : CVectorTemplate<T>(4,0)
00058 { }
00059
00060
00061
00062 CQuaternion(const T &r,const T &x,const T &y,const T &z) : CVectorTemplate<T>(4)
00063 {
00064 (*this)[0] = r;
00065 (*this)[1] = x;
00066 (*this)[2] = y;
00067 (*this)[3] = z;
00068 }
00069
00070
00071
00072 CQuaternion(const CQuaternion &qin) : CVectorTemplate<T>(qin)
00073 { }
00074
00075
00076
00077 CQuaternion(const CVectorTemplate<T> &in) : CVectorTemplate<T>(4)
00078 {
00079 if (in.size()!=3)
00080 THROW_EXCEPTION("Wrong Dimension in input vector for quaternion Constructor");
00081
00082 const T x = in[0];
00083 const T y = in[1];
00084 const T z = in[2];
00085 if ((x==0)&&(y==0)&&(z==0))
00086 {
00087 (*this)[0] = 1;
00088 (*this)[1] = 0;
00089 (*this)[2] = 0;
00090 (*this)[3] = 0;
00091 }
00092 else
00093 {
00094 const T angle = sqrt(x*x+y*y+z*z);
00095 const T s = (sin(angle/2))/angle;
00096 const T c = cos(angle/2);
00097 (*this)[0] = c;
00098 (*this)[1] = x * s;
00099 (*this)[2] = y * s;
00100 (*this)[3] = z * s;
00101 }
00102 }
00103
00104
00105
00106
00107
00108 T r()const {return (*this)[0];}
00109 T x()const {return (*this)[1];}
00110 T y()const {return (*this)[2];}
00111 T z()const {return (*this)[3];}
00112 void r(const T &r) {(*this)[0]=r;}
00113 void x(const T &x) {(*this)[1]=x;}
00114 void y(const T &y) {(*this)[2]=y;}
00115 void z(const T &z) {(*this)[3]=z;}
00116
00117
00118
00119 void q_conv(const CVectorTemplate<T> &in)
00120 {
00121 ASSERT_(in.size()==3);
00122 const T x = in[0];
00123 const T y = in[1];
00124 const T z = in[2];
00125 const T angle = sqrt(x*x+y*y+z*z);
00126 const T s = (sin(angle/2))/angle;
00127 const T c = cos(angle/2);
00128 (*this)[0] = c;
00129 (*this)[1] = x * s;
00130 (*this)[2] = y * s;
00131 (*this)[3] = z * s;
00132 }
00133
00134
00135
00136
00137 void q_prod(const CQuaternion &qin1, const CQuaternion &qin2)
00138 {
00139 T q1r = qin1.r();
00140 T q1x = qin1.x();
00141 T q1y = qin1.y();
00142 T q1z = qin1.z();
00143
00144 T q2r = qin2.r();
00145 T q2x = qin2.x();
00146 T q2y = qin2.y();
00147 T q2z = qin2.z();
00148
00149 (*this)[0] = q1r*q2r - q1x*q2x - q1y*q2y - q1z*q2z;
00150 (*this)[1] = q1r*q2x + q2r*q1x + q1y*q2z - q2y*q1z;
00151 (*this)[2] = q1r*q2y + q2r*q1y + q1z*q2x - q2z*q1x;
00152 (*this)[3] = q1r*q2z + q2r*q1z + q1x*q2y - q2x*q1y;
00153
00154
00155 q_normalise();
00156 }
00157
00158
00159
00160
00161 void q_normalise()
00162 {
00163 T qq = 1.0/sqrt( square((*this)[0])+square((*this)[1])+square((*this)[2])+square((*this)[3]));
00164
00165 for (unsigned int i=0;i<4;i++)
00166 (*this)[i] *= qq;
00167 }
00168
00169
00170
00171 void q_normJac(CMatrixTemplateNumeric<T> &J) const
00172 {
00173 T r_=(*this)[0];
00174 T x_=(*this)[1];
00175 T y_=(*this)[2];
00176 T z_=(*this)[3];
00177 T n = r_*r_ + x_*x_ + y_*y_ + z_*z_;
00178 n = 1 / (n*sqrt(n));
00179
00180 J.setSize(4,4);
00181 J(0,0)=x_*x_+y_*y_+z_*z_; J(0,1)=-r_*x_; J(0,2)=-r_*y_; J(0,3)=-r_*z_;
00182 J(1,0)=-x_*r_; J(1,1)=r_*r_+y_*y_+z_*z_; J(1,2)=-x_*y_; J(1,3)=-x_*z_;
00183 J(2,0)=-y_*r_; J(2,1)=-y_*x_; J(2,2)=r_*r_+x_*x_+z_*z_; J(2,3)=-y_*z_;
00184 J(3,0)=-z_*r_; J(3,1)=-z_*x_; J(3,2)=-z_*y_; J(3,3)=r_*r_+x_*x_+y_*y_;
00185 J *=n;
00186 }
00187
00188
00189
00190 void q_rotation_matrix(CMatrixTemplateNumeric<T> &M) const
00191 {
00192 M.setSize(3,3);
00193 T r_=(*this)[0];
00194 T x_=(*this)[1];
00195 T y_=(*this)[2];
00196 T z_=(*this)[3];
00197 M.setSize(3,3);
00198 M(0,0)=r_*r_+x_*x_-y_*y_-z_*z_; M(0,1)=2*(x_*y_ -r_*z_); M(0,2)=2*(z_*x_+r_*y_);
00199 M(1,0)=2*(x_*y_+r_*z_); M(1,1)=r_*r_-x_*x_+y_*y_-z_*z_; M(1,2)=2*(y_*z_-r_*x_);
00200 M(2,0)=2*(z_*x_-r_*y_); M(2,1)=2*(y_*z_+r_*x_); M(2,2)=r_*r_-x_*x_-y_*y_+z_*z_;
00201
00202 }
00203
00204
00205
00206 CQuaternion q_conj() const
00207 {
00208 CQuaternion q_aux;
00209 q_aux[0]=(*this)[0];
00210 q_aux[1]=-(*this)[1];
00211 q_aux[2]=-(*this)[2];
00212 q_aux[3]=-(*this)[3];
00213 return q_aux;
00214 }
00215
00216
00217
00218 void rpy(T &r, T &p, T &y) const
00219 {
00220
00221
00222 T r_=(*this)[0];
00223 T x_=(*this)[1];
00224 T y_=(*this)[2];
00225 T z_=(*this)[3];
00226 r=atan2(2*(y_*z_+r_*x_),r_*r_-x_*x_-y_*y_+z_*z_);
00227 p=asin(-2*(x_*z_-r_*y_));
00228 y=atan2(2*(x_*y_+r_*z_),r_*r_+x_*x_-y_*y_-z_*z_);
00229 }
00230
00231 CQuaternion operator * (const T &factor)
00232 {
00233 return (*this)*factor;
00234 }
00235
00236 };
00237
00238
00239 typedef CQuaternion<double> CQuaternionDouble;
00240 typedef CQuaternion<float> CQuaternionFloat;
00241
00242 }
00243
00244 }
00245
00246 #endif