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 template <class T>
00042 class CQuaternion:public CVectorTemplate<T>
00043 {
00047 public:
00048
00049
00050
00051
00055 CQuaternion()
00056 {
00057 (*this).resize(4);
00058 }
00059
00063 CQuaternion(const T &r,const T &x,const T &y,const T &z)
00064 {
00065 (*this).resize(4);
00066 (*this)[0] = r;
00067 (*this)[1] = x;
00068 (*this)[2] = y;
00069 (*this)[3] = z;
00070 }
00071
00075 CQuaternion(const CQuaternion &qin)
00076 {
00077 (*this).resize(4);
00078 (*this)=qin;
00079 }
00080
00084 CQuaternion(const CVectorTemplate<T> &in)
00085 {
00086
00087 if (in.size()!=3)
00088 THROW_EXCEPTION("Wrong Dimension in input vector for quaternion Constructor");
00089
00090 (*this).resize(4);
00091 const T x = in[0];
00092 const T y = in[1];
00093 const T z = in[2];
00094 if ((x==0)&&(y==0)&&(z==0))
00095 {
00096 (*this)[0] = 1;
00097 (*this)[1] = 0;
00098 (*this)[2] = 0;
00099 (*this)[3] = 0;
00100 }
00101 else
00102 {
00103 const T angle = sqrt(x*x+y*y+z*z);
00104 const T s = (sin(angle/2))/angle;
00105 const T c = cos(angle/2);
00106 (*this)[0] = c;
00107 (*this)[1] = x * s;
00108 (*this)[2] = y * s;
00109 (*this)[3] = z * s;
00110 }
00111 }
00112
00113
00114
00115
00119 T r()const {return (*this)[0];}
00123 T x()const {return (*this)[1];}
00127 T y()const {return (*this)[2];}
00131 T z()const {return (*this)[3];}
00135 void r(const T &r) {(*this)[0]=r;}
00139 void x(const T &x) {(*this)[1]=x;}
00143 void y(const T &y) {(*this)[2]=y;}
00147 void z(const T &z) {(*this)[3]=z;}
00148
00152 void q_conv(const CVectorTemplate<T> &in)
00153 {
00154 ASSERT_(in.size()==3);
00155 const T x = in[0];
00156 const T y = in[1];
00157 const T z = in[2];
00158 const T angle = sqrt(x*x+y*y+z*z);
00159 const T s = (sin(angle/2))/angle;
00160 const T c = cos(angle/2);
00161 (*this)[0] = c;
00162 (*this)[1] = x * s;
00163 (*this)[2] = y * s;
00164 (*this)[3] = z * s;
00165 }
00166
00170 void q_prod(const CQuaternion &qin1, const CQuaternion &qin2)
00171 {
00172 T q1r = qin1.r();
00173 T q1x = qin1.x();
00174 T q1y = qin1.y();
00175 T q1z = qin1.z();
00176
00177 T q2r = qin2.r();
00178 T q2x = qin2.x();
00179 T q2y = qin2.y();
00180 T q2z = qin2.z();
00181
00182 (*this)[0] = q1r*q2r - q1x*q2x - q1y*q2y - q1z*q2z;
00183 (*this)[1] = q1r*q2x + q2r*q1x + q1y*q2z - q2y*q1z;
00184 (*this)[2] = q1r*q2y + q2r*q1y + q1z*q2x - q2z*q1x;
00185 (*this)[3] = q1r*q2z + q2r*q1z + q1x*q2y - q2x*q1y;
00186
00187
00188 T qq = sqrt((*this)[0]*(*this)[0]+(*this)[1]*(*this)[1]+(*this)[2]*(*this)[2]+(*this)[3]*(*this)[3]);
00189
00190 for (int i=0;i<4;i++)
00191 (*this)[i]=(*this)[i] / qq;
00192 }
00193
00197 void q_normalise()
00198 {
00199 T qq = sqrt((*this)[0]*(*this)[0]+(*this)[1]*(*this)[1]+(*this)[2]*(*this)[2]+(*this)[3]*(*this)[3]);
00200
00201 for (int i=0;i<4;i++)
00202 (*this)[i]=(*this)[i] / qq;
00203 }
00204
00208 void q_normJac(CMatrixTemplateNumeric<T> &J)
00209 {
00210 T r_=(*this)[0];
00211 T x_=(*this)[1];
00212 T y_=(*this)[2];
00213 T z_=(*this)[3];
00214 T n = r_*r_ + x_*x_ + y_*y_ + z_*z_;
00215 n = 1 / (n*sqrt(n));
00216
00217 J.setSize(4,4);
00218 J(0,0)=x_*x_+y_*y_+z_*z_; J(0,1)=-r_*x_; J(0,2)=-r_*y_; J(0,3)=-r_*z_;
00219 J(1,0)=-x_*r_; J(1,1)=r_*r_+y_*y_+z_*z_; J(1,2)=-x_*y_; J(1,3)=-x_*z_;
00220 J(2,0)=-y_*r_; J(2,1)=-y_*x_; J(2,2)=r_*r_+x_*x_+z_*z_; J(2,3)=-y_*z_;
00221 J(3,0)=-z_*r_; J(3,1)=-z_*x_; J(3,2)=-z_*y_; J(3,3)=r_*r_+x_*x_+y_*y_;
00222 J *=n;
00223
00224 }
00225
00229 void q_rotation_matrix(CMatrixTemplateNumeric<T> &M)
00230 {
00231 M.setSize(3,3);
00232 T r_=(*this)[0];
00233 T x_=(*this)[1];
00234 T y_=(*this)[2];
00235 T z_=(*this)[3];
00236 M.setSize(3,3);
00237 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_);
00238 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_);
00239 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_;
00240
00241 }
00242
00246 CQuaternion q_conj()
00247 {
00248 CQuaternion q_aux;
00249 q_aux[0]=(*this)[0];
00250 q_aux[1]=-(*this)[1];
00251 q_aux[2]=-(*this)[2];
00252 q_aux[3]=-(*this)[3];
00253 return q_aux;
00254 }
00255
00259 void rpy(T &r, T &p, T &y)
00260 {
00261 T r_=(*this)[0];
00262 T x_=(*this)[1];
00263 T y_=(*this)[2];
00264 T z_=(*this)[3];
00265 r=atan2(2*(y_*z_+r_*x_),r_*r_-x_*x_-y_*y_+z_*z_);
00266 p=asin(-2*(x_*z_-r_*y_));
00267 y=atan2(2*(x_*y_+r_*z_),r_*r_+x_*x_-y_*y_-z_*z_);
00268 }
00269
00270 CQuaternion operator * (const T &factor)
00271 {
00272 return (*this)*factor;
00273 }
00274
00275 };
00276
00277 }
00278
00279 }
00280
00281 #endif