Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #ifndef _TRANSFORMATION_H_
00018 #define _TRANSFORMATION_H_
00019 #include "rotation_matrix.h"
00020 #include "angle.h"
00021 #include "quaternion.h"
00022 #include "axis_angle.h"
00023
00026
00034 template <typename Rotation>
00035 struct _Transformation {
00036 typedef typename Rotation::BaseType BaseType;
00037 typedef Rotation RotationType;
00038 typedef _Vector<Rotation::Dimension, BaseType> TranslationType;
00039 typedef _Matrix<Rotation::Dimension+1, Rotation::Dimension+1, BaseType> TransformationMatrix;
00040 typedef _Vector<Rotation::Dimension+Rotation::Angles, BaseType> TransformationVector;
00041
00042 _Transformation() {
00043 _translation.fill(0.0);
00044 }
00045
00046 _Transformation(const TranslationType& t, const Rotation& r) {
00047 _translation=t;
00048 _rotation=r;
00049 }
00050
00051 TranslationType& translation() {return _translation;}
00052
00053 const TranslationType& translation() const {return _translation;}
00054
00055 Rotation& rotation() {return _rotation;}
00056
00057 const Rotation& rotation() const {return _rotation;}
00058
00059 _Transformation<Rotation> operator * (const _Transformation<Rotation>& t) const {
00060 return _Transformation<Rotation>(translation()+rotation()*t.translation(), rotation()*t.rotation());
00061 }
00062
00063 _Transformation<Rotation>& operator *= (const _Transformation<Rotation>& t) {
00064 _translation+=rotation()*t.translation();
00065 _rotation*=t.rotation();
00066 return *this;
00067 }
00068
00069 _Transformation<Rotation> inverse() const {
00070 RotationType _inverse=_rotation.inverse();
00071 return _Transformation<Rotation>(_inverse*(_translation*BaseType(-1.)), _inverse);
00072 }
00073
00074 TranslationType operator* (const TranslationType& t) const {
00075 return TranslationType(translation()+rotation()*t);
00076 }
00077
00078 TransformationMatrix toMatrix() const {
00079 TransformationMatrix m= TransformationMatrix::eye(1.);
00080 _Matrix <Rotation::Dimension, Rotation::Dimension, BaseType> r = _rotation.rotationMatrix();
00081 for (int i=0; i<r.rows(); i++)
00082 for (int j=0; j<r.cols(); j++)
00083 m[i][j]=r[i][j];
00084 for (int i=0; i<_translation.size(); i++)
00085 m[i][m.cols()-1]=_translation[i];
00086 return m;
00087 }
00088
00089 TransformationVector toVector() const {
00090 TransformationVector v;
00091 for (int i=0; i<_translation.size(); i++)
00092 v[i]=_translation[i];
00093 _Vector<RotationType::Angles, BaseType> a=_rotation.angles();
00094 for (int i=0; i<a.size(); i++)
00095 v[i+RotationType::Dimension]=a[i];
00096 return v;
00097 }
00098
00099 static _Transformation<Rotation> fromVector(const TransformationVector& v) {
00100 TranslationType translation_;
00101 _Vector<RotationType::Angles, BaseType> a;
00102 for (int i=0; i<RotationType::Dimension; i++)
00103 translation_[i]=v[i];
00104 for (int i=0; i<a.size(); i++)
00105 a[i]=v[i+RotationType::Dimension];
00106 RotationType rotation_(a);
00107 return _Transformation<Rotation>(translation_,rotation_);
00108 }
00109
00110 TranslationType _translation;
00111 RotationType _rotation;
00112 };
00113
00114 typedef _Transformation< RotationMatrix2f> _Transformation2rf;
00115 typedef _Transformation< Anglef> _Transformation2f;
00116 typedef _Transformation< RotationMatrix2> _Transformation2r;
00117 typedef _Transformation< Angle> _Transformation2;
00118
00119 typedef _Transformation< RotationMatrix3f> Transformation3rf;
00120 typedef _Transformation< Quaternionf> Transformation3f;
00121 typedef _Transformation< AxisAnglef> Transformation3af;
00122
00123 typedef _Transformation< Quaternion> Transformation3;
00124 typedef _Transformation< RotationMatrix3> Transformation3r;
00125 typedef _Transformation< AxisAngle> Transformation3a;
00126
00127 typedef _Transformation< Angle> Transformation2;
00128
00130
00131
00132
00133
00134
00135 template <typename Base>
00136 _Transformation< _Quaternion<Base> > manifold2Transformation(const typename _Transformation< _Quaternion<Base> >::TransformationVector& v) {
00137 _Vector<3, Base> t;
00138 t[0] = v[0];
00139 t[1] = v[1];
00140 t[2] = v[2];
00141 _AxisAngle<Base> a;
00142 a[0]=v[3];
00143 a[1]=v[4];
00144 a[2]=v[5];
00145 return _Transformation< _Quaternion<Base> >(t,a.quaternion());
00146 }
00147
00148 template <typename Base>
00149 typename _Transformation< _Quaternion<Base> >::TransformationVector transformation2manifold(const _Transformation< _Quaternion<Base> >& t) {
00150 typename _Transformation< _Quaternion<Base> >::TransformationVector v;
00151 v[0] = t.transformation()[0];
00152 v[1] = t.transformation()[1];
00153 v[2] = t.transformation()[2];
00154 _AxisAngle<Base> a(t.rotation());
00155 v[3]=a[3];
00156 v[4]=a[4];
00157 v[5]=a[5];
00158 return v;
00159 }
00160
00161
00162
00163 #endif