angle.h
Go to the documentation of this file.
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 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


hogman_minimal
Author(s): Maintained by Juergen Sturm
autogenerated on Wed Dec 26 2012 15:36:47