00001 // HOG-Man - Hierarchical Optimization for Pose Graphs on Manifolds 00002 // Copyright (C) 2010 G. Grisetti, R. Kümmerle, C. Stachniss 00003 // 00004 // HOG-Man is free software: you can redistribute it and/or modify 00005 // it under the terms of the GNU Lesser General Public License as published 00006 // by the Free Software Foundation, either version 3 of the License, or 00007 // (at your option) any later version. 00008 // 00009 // HOG-Man is distributed in the hope that it will be useful, 00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of 00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00012 // GNU Lesser General Public License for more details. 00013 // 00014 // You should have received a copy of the GNU Lesser General Public License 00015 // along with this program. If not, see <http://www.gnu.org/licenses/>. 00016 00017 #ifndef _ANGLE_H_ 00018 #define _ANGLE_H_ 00019 00020 #include "rotation_matrix.h" 00023 00029 template <typename Base=double> 00030 struct _Angle { 00031 static const int Dimension=2; 00032 static const int Angles=1; 00033 typedef Base BaseType; 00035 _Angle(Base a=Base(0.)){ 00036 _angle=a; 00037 _normalize(); 00038 } 00039 00041 _Angle(const _Vector<1, Base>& a){ 00042 _angle=a[0]; 00043 _normalize(); 00044 } 00045 00048 _Angle<Base> operator * ( const _Angle<Base>& a) const { 00049 return _Angle<Base>(_angle+a._angle); 00050 } 00051 00053 _Angle<Base>& operator *= ( const _Angle<Base>& a) { 00054 _angle+=a._angle; 00055 _normalize(); 00056 return *this; 00057 } 00058 00060 _Vector<2,Base> operator * (const _Vector<2,Base>& v) const { 00061 return rotationMatrix()*v; 00062 } 00063 00065 _Angle<Base> inverse() const { 00066 return _Angle<Base>(-_angle); 00067 } 00068 00070 _RotationMatrix2<Base> rotationMatrix() const { 00071 return _RotationMatrix2<Base>(_angle); 00072 } 00073 00075 inline Base angle() const {return _angle;} 00076 00080 _Vector<1, Base> angles() const {_Vector<1, Base> v; v[0]=_angle; return v;} 00081 00082 Base _angle; 00083 void _normalize() { 00084 if (_angle >= Base(0)) 00085 _angle= fmod((Base)(_angle + Base(M_PI)),(Base)(2*M_PI)) - M_PI; 00086 else 00087 _angle= -(fmod((Base)(-_angle + M_PI),(Base)(2*M_PI)) - M_PI); 00088 } 00089 00091 operator Base () const { 00092 return _angle; 00093 } 00094 }; 00096 00097 typedef _Angle<float> Anglef; 00098 typedef _Angle<double> Angle; 00099 00100 #endif 00101