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 #ifndef _TRANSFORMATION3_HXX_
00038 #define _TRANSFORMATION3_HXX_
00039
00040 #include <assert.h>
00041 #include <cmath>
00042 #include "dmatrix.hh"
00043
00044 namespace AISNavigation {
00045
00046 template <class T>
00047 struct Vector3 {
00048 T elems[3] ;
00049
00050 Vector3(T x, T y, T z) {elems[0]=x; elems[1]=y; elems[2]=z;}
00051 Vector3() {elems[0]=0.; elems[1]=0.; elems[2]=0.;}
00052 Vector3(const DVector<T>& t){}
00053
00054
00055 inline const T& x() const {return elems[0];}
00056 inline const T& y() const {return elems[1];}
00057 inline const T& z() const {return elems[2];}
00058 inline T& x() {return elems[0];}
00059 inline T& y() {return elems[1];}
00060 inline T& z() {return elems[2];}
00061
00062
00063 inline const T& roll() const {return elems[0];}
00064 inline const T& pitch() const {return elems[1];}
00065 inline const T& yaw() const {return elems[2];}
00066 inline T& roll() {return elems[0];}
00067 inline T& pitch() {return elems[1];}
00068 inline T& yaw() {return elems[2];}
00069
00070 };
00071
00072 template <class T>
00073 struct Pose3 : public DVector<T>{
00074 Pose3();
00075 Pose3(const Vector3<T>& rot, const Vector3<T>& trans);
00076 Pose3(const T& x, const T& y, const T& z, const T& roll, const T& pitch, const T& yaw);
00077 Pose3(const DVector<T>& v): DVector<T>(v) {assert(v.dim()==6);}
00078
00079 inline operator const DVector<T>& () {return (const DVector<T>)*this;}
00080 inline operator DVector<T>& () {return *this;}
00081
00082 inline const T& roll() const {return DVector<T>::elems[0];}
00083 inline const T& pitch() const {return DVector<T>::elems[1];}
00084 inline const T& yaw() const {return DVector<T>::elems[2];}
00085 inline const T& x() const {return DVector<T>::elems[3];}
00086 inline const T& y() const {return DVector<T>::elems[4];}
00087 inline const T& z() const {return DVector<T>::elems[5];}
00088
00089 inline T& roll() {return DVector<T>::elems[0];}
00090 inline T& pitch() {return DVector<T>::elems[1];}
00091 inline T& yaw() {return DVector<T>::elems[2];}
00092 inline T& x() {return DVector<T>::elems[3];}
00093 inline T& y() {return DVector<T>::elems[4];}
00094 inline T& z() {return DVector<T>::elems[5];}
00095
00096 };
00097
00098
00104 template <class T>
00105 struct Quaternion{
00106
00110 Quaternion();
00111
00115 Quaternion(const Vector3<T>& pose);
00116
00120 Quaternion(const T _w, const T _x, const T _y, const T _z);
00121
00125 Quaternion(const T _roll_x_phi, const T _pitch_y_theta, const T _yaw_z_psi);
00126
00130 inline Quaternion<T> conjugated() const;
00131
00135 inline Quaternion<T> normalized() const;
00136
00140 inline Quaternion<T> inverse() const;
00141
00142
00143 inline Quaternion(const Vector3<T>& axis, const T& angle);
00144
00151 inline Quaternion<T> rotateThisAlong (const Vector3<T>& axis, const T alpha) const;
00152
00153
00160 inline Quaternion<T> rotatePoint(const Quaternion& p) const;
00161
00168 inline Vector3<T> rotatePoint(const Vector3<T>& p) const;
00169
00176 inline Quaternion withRotation (const T alpha) const;
00177
00182 inline Vector3<T> toAngles() const;
00183
00184 inline Vector3<T> axis() const;
00185 inline T angle() const;
00186
00187
00191 inline T norm() const;
00192
00196 inline T re() const;
00197
00201 inline Vector3<T> im() const;
00202
00203 T w,x,y,z;
00204 };
00205
00206
00207
00208 template <class T> inline Quaternion<T> operator + (const Quaternion<T> & left, const Quaternion<T>& right);
00209 template <class T> inline Quaternion<T> operator - (const Quaternion<T> & left, const Quaternion<T>& right);
00210 template <class T> inline Quaternion<T> operator * (const Quaternion<T> & left, const Quaternion<T>& right);
00211 template <class T> inline Quaternion<T> operator * (const Quaternion<T> & left, const T scalar);
00212 template <class T> inline Quaternion<T> operator * (const T scalar, const Quaternion<T>& right);
00213 template <class T> std::ostream& operator << (std::ostream& os, const Quaternion<T>& q);
00214
00215 template <class T> inline T innerproduct(const Quaternion<T>& left, const Quaternion<T>& right);
00216 template <class T> inline Quaternion<T> slerp(const Quaternion<T>& from, const Quaternion<T>& to, const T lambda);
00217
00218
00219
00220 template <class T>
00221 struct Transformation3{
00222 Quaternion<T> rotationQuaternion;
00223 Vector3<T> translationVector;
00224
00225 Transformation3(){}
00226
00227 inline static Transformation3<T> identity();
00228
00229 Transformation3 (const Vector3<T>& trans, const Quaternion<T>& rot);
00230 Transformation3 (const Pose3<T>& v);
00231 Transformation3 (const T& x, const T& y, const T& z, const T& roll, const T& pitch, const T& yaw);
00232
00233 inline Vector3<T> translation() const;
00234 inline Quaternion <T> rotation() const;
00235
00236 inline Pose3<T> toPoseType() const;
00237
00238 inline void setTranslation(const Vector3<T>& t);
00239 inline void setTranslation(const T& x, const T& y, const T& z);
00240
00241 inline void setRotation(const Vector3<T>& r);
00242 inline void setRotation(const T& roll, const T& pitch, const T& yaw);
00243 inline void setRotation(const Quaternion<T>& q);
00244
00245
00246 inline Transformation3<T> inv() const;
00247 inline bool validRotation(const T& epsilon=0.001) const;
00248
00249 };
00250
00251 template <class T>
00252 inline Vector3<T> operator * (const Transformation3<T>& m, const Vector3<T>& v);
00253
00254 template <class T>
00255 inline Transformation3<T> operator * (const Transformation3<T>& m1, const Transformation3<T>& m2);
00256
00257 template <class T>
00258 struct Operations3D{
00259 typedef T BaseType;
00260 typedef Pose3<T> PoseType;
00261 typedef Quaternion<T> RotationType;
00262 typedef Vector3<T> TranslationType;
00263 typedef Transformation3<T> TransformationType;
00264 typedef DMatrix<T> CovarianceType;
00265 typedef DMatrix<T> InformationType;
00266 typedef Transformation3<T> ParametersType;
00267 };
00268
00269 }
00270
00271
00272 #include "transformation3.hxx"
00273
00274 #endif
00275