angle.h

Go to the documentation of this file.

// HOG-Man - Hierarchical Optimization for Pose Graphs on Manifolds
// Copyright (C) 2010 G. Grisetti, R. Kümmerle, C. Stachniss
// 
// HOG-Man is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published
// by the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
// 
// HOG-Man is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
// 
// You should have received a copy of the GNU Lesser General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

#ifndef _ANGLE_H_
#define _ANGLE_H_

#include "rotation_matrix.h"

template <typename Base=double>
struct _Angle {
  static const int Dimension=2;
  static const int Angles=1;
  typedef Base BaseType;
  _Angle(Base a=Base(0.)){
    _angle=a;
    _normalize();
  }

  _Angle(const _Vector<1, Base>& a){
    _angle=a[0];
    _normalize();
  }

  _Angle<Base> operator  * ( const _Angle<Base>& a) const {
    return _Angle<Base>(_angle+a._angle);
  }

  _Angle<Base>& operator *= ( const _Angle<Base>& a) {
    _angle+=a._angle;
    _normalize();
    return *this;
  }

  _Vector<2,Base> operator * (const _Vector<2,Base>& v) const {
    return rotationMatrix()*v;
  }

  _Angle<Base> inverse() const {
    return _Angle<Base>(-_angle);
  }

  _RotationMatrix2<Base> rotationMatrix() const {
    return _RotationMatrix2<Base>(_angle);
  }

  inline Base angle() const {return _angle;}

  _Vector<1, Base> angles() const {_Vector<1, Base> v; v[0]=_angle; return v;}

  Base _angle;
  void _normalize() {
    if (_angle >= Base(0))
      _angle= fmod((Base)(_angle + Base(M_PI)),(Base)(2*M_PI)) - M_PI;
    else
      _angle= -(fmod((Base)(-_angle + M_PI),(Base)(2*M_PI)) - M_PI);
  }

  operator Base () const  {
    return _angle;
  }
};

typedef _Angle<float>  Anglef;
typedef _Angle<double> Angle;

#endif