angle_deg.h

Go to the documentation of this file.

// -*-c++-*-

 
/*
 *Copyright:
 
 Copyright (C) Hidehisa Akiyama
 
 This code 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.
 
 This library 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 library; if not, write to the Free Software
 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 
 *EndCopyright:
 */

 
#ifndef RCSC_GEOM_ANGLEDEG_H
#define RCSC_GEOM_ANGLEDEG_H
 
#include <functional>
#include <iostream>
#include <cmath>
 
namespace rcsc {

class AngleDeg {
    // : public boost::addable< AngleDeg >
    // , public boost::addable2< AngleDeg, double >
    // , public boost::addable2_left< double,  AngleDeg >
    // , public boost::subtractable< AngleDeg >
    // , public boost::subtractable2< AngleDeg, double >
    // , public boost::subtractable2_left< double, AngleDeg >
    // , public boost::multipliable2< AngleDeg, double >
    // , public boost::dividable2< AngleDeg, double >
private:
    double M_degree;
 
public:
    static const double EPSILON;

    static const double PI;
    static const double TWO_PI;
    static const double DEG2RAD;
    static const double RAD2DEG;

    AngleDeg()
        : M_degree( 0.0 )
      { }

    AngleDeg( const double & deg )
        : M_degree( deg )
      {
          normalize();
      }

    const
    AngleDeg & operator=( const double & deg )
      {
          M_degree = deg;
          return normalize();
      }
 
private:
    const
    AngleDeg & normalize()
      {
          if ( M_degree < -360.0 || 360.0 < M_degree )
          {
              M_degree = std::fmod( M_degree, 360.0 );
          }
 
          if ( M_degree < -180.0 )
          {
              M_degree += 360.0;
          }
 
          if ( M_degree > 180.0 )
          {
              M_degree -= 360.0;
          }
 
          return *this;
      }
 
public:

    const
    double & degree() const
      {
          return M_degree;
      }

    double abs() const
      {
          return std::fabs( degree() );
      }

    double radian() const
      {
          return degree() * DEG2RAD;
      }
 
    /*
      operator double() const
      {
      return degree();
      }
    */

    AngleDeg operator-() const
      {
          return AngleDeg( - degree() );
      }

    const
    AngleDeg & operator+=( const AngleDeg & angle )
      {
          M_degree += angle.degree();
          return normalize();
      }

    const
    AngleDeg & operator+=( const double & deg )
      {
          M_degree += deg;
          return normalize();
      }

    const
    AngleDeg & operator-=( const AngleDeg & angle )
      {
          M_degree -= angle.degree();
          return normalize();
      }

    const
    AngleDeg & operator-=( const double & deg )
      {
          M_degree -= deg;
          return normalize();
      }

    const
    AngleDeg & operator*=( const double & scalar )
      {
          M_degree *= scalar;
          return normalize();
      }

    const
    AngleDeg & operator/=( const double & scalar )
      {
          if ( std::fabs( scalar ) < EPSILON )
          {
              return * this;
          }
          M_degree /= scalar;
          return normalize();
      }


    bool isLeftOf( const AngleDeg & angle ) const
      {
          //return (*this - angle).degree() < 0.0;
          double diff = angle.degree() - this->degree();
          return ( ( 0.0 < diff && diff < 180.0 )
                   || diff < -180.0 );
      }

    bool isLeftEqualOf( const AngleDeg & angle ) const
      {
          //return (*this - angle).degree() <= 0.0;
          double diff = angle.degree() - this->degree();
          return ( ( 0.0 <= diff && diff < 180.0 )
                   || diff < -180.0 );
      }

    bool isRightOf( const AngleDeg & angle ) const
      {
          //return (*this - angle).degree() > 0.0;
          double diff = this->degree() - angle.degree();
          return ( ( 0.0 < diff && diff < 180.0 )
                   || diff < -180.0 );
      }

    bool isRightEqualOf( const AngleDeg & angle ) const
      {
          //return (*this - angle).degree() >= 0.0;
          double diff = this->degree() - angle.degree();
          return ( ( 0.0 <= diff && diff < 180.0 )
                   || diff < -180.0 );
      }

    double cos() const
      {
          return std::cos( degree() * DEG2RAD );
      }

    double sin() const
      {
          return std::sin( degree() * DEG2RAD );
      }

    double tan() const
      {
          return std::tan( degree() * DEG2RAD );
      }

    bool isWithin( const AngleDeg & left,
                   const AngleDeg & right ) const;

    void sinMinMax( const double & angle_err,
                    double * minsin,
                    double * maxsin ) const;

    void cosMinMax( const double & angle_err,
                    double * mincos,
                    double * maxcos ) const;

    // static utility method concerned with angle operation

    inline
    static
    double normalize_angle( double dir )
      {
          if ( dir < -360.0 || 360.0 < dir )
          {
              dir = std::fmod( dir, 360.0 );
          }
 
          if ( dir < -180.0 )
          {
              dir += 360.0;
          }
 
          if ( dir > 180.0 )
          {
              dir -= 360.0;
          }
 
          return dir;
      }

    inline
    static
    double rad2deg( const double & rad )
      {
          return rad * RAD2DEG;
      }

    inline
    static
    double deg2rad( const double & deg )
      {
          return deg * DEG2RAD;
      }

    inline
    static
    double cos_deg( const double & deg )
      {
          return std::cos( deg2rad( deg ) );
      }

    inline
    static
    double sin_deg( const double & deg )
      {
          return std::sin( deg2rad( deg ) );
      }

    inline
    static
    double tan_deg( const double & deg )
      {
          return std::tan( deg2rad( deg ) );
      }

    inline
    static
    double acos_deg( const double & cosine )
      {
          return ( cosine >= 1.0
                   ? 0.0
                   : ( cosine <= -1.0
                       ? 180.0
                       : rad2deg( std::acos( cosine ) ) ) );
      }

    inline
    static
    double asin_deg( const double & sine )
      {
          return ( sine >= 1.0
                   ? 90.0
                   : ( sine <= -1.0
                       ? -90.0
                       : rad2deg( std::asin( sine ) ) ) );
      }

    inline
    static
    double atan_deg( const double & tangent )
      {
          return rad2deg( std::atan( tangent ) );
      }

    inline
    static
    double atan2_deg( const double & y,
                      const double & x )
      {
          return ( ( x == 0.0 && y == 0.0 )
                   ? 0.0
                   : rad2deg( std::atan2( y, x ) ) );
      }

    static
    AngleDeg bisect( const AngleDeg & left,
                     const AngleDeg & right );


    std::ostream & print( std::ostream & os ) const
      {
          return os << degree();
      }

    std::ostream & printRound( std::ostream & os,
                               const double & step = 0.1 ) const
      {
          return os << rint( degree() / step ) * step;
      }


    class DegreeCmp
        : public std::binary_function< AngleDeg, AngleDeg, bool > {
    public:
        result_type operator()( const first_argument_type & lhs,
                                const second_argument_type & rhs ) const
          {
              return lhs.degree() < rhs.degree();
          }
    };
 
};
 


class AngleIsWithin
    : public std::unary_function< AngleDeg, bool > {
    const AngleDeg M_left; 
    const AngleDeg M_right; 
public:
    AngleIsWithin( const AngleDeg & left,
                   const AngleDeg & right )
        : M_left( left )
        , M_right( right )
      { }

    result_type operator()( const argument_type & angle ) const
      {
          return angle.isWithin( M_left, M_right );
      }
};
 
 
} // end of namespace
 

// arith operators
 
/*-------------------------------------------------------------------*/
inline
const
rcsc::AngleDeg
operator+( const rcsc::AngleDeg & lhs,
           const rcsc::AngleDeg & rhs )
{
    return rcsc::AngleDeg( lhs ) += rhs;
}
 
/*-------------------------------------------------------------------*/
inline
const
rcsc::AngleDeg
operator+( const rcsc::AngleDeg & lhs,
           const double & rhs )
{
    return rcsc::AngleDeg( lhs ) += rhs;
}
 
/*-------------------------------------------------------------------*/
inline
const
rcsc::AngleDeg
operator+( const double & lhs,
           const rcsc::AngleDeg & rhs )
{
    return rcsc::AngleDeg( rhs ) += lhs;
}
 
/*-------------------------------------------------------------------*/
inline
const
rcsc::AngleDeg
operator-( const rcsc::AngleDeg & lhs,
           const rcsc::AngleDeg & rhs )
{
    return rcsc::AngleDeg( lhs ) -= rhs;
}
 
/*-------------------------------------------------------------------*/
inline
const
rcsc::AngleDeg
operator-( const rcsc::AngleDeg & lhs,
           const double & rhs )
{
    return rcsc::AngleDeg( lhs ) -= rhs;
}
 
/*-------------------------------------------------------------------*/
inline
const
rcsc::AngleDeg
operator-( const double & lhs,
           const rcsc::AngleDeg & rhs )
{
    return rcsc::AngleDeg( lhs ) -= rhs;
}

// predicate operator
 
/*-------------------------------------------------------------------*/
inline
bool
operator!=( const rcsc::AngleDeg & lhs,
            const rcsc::AngleDeg & rhs )
{
    return std::fabs( lhs.degree() - rhs.degree() ) > rcsc::AngleDeg::EPSILON;
}
 
/*-------------------------------------------------------------------*/
inline
bool
operator!=( const rcsc::AngleDeg & lhs,
            const double & rhs )
{
    return std::fabs( lhs.degree() - rhs ) > rcsc::AngleDeg::EPSILON;
}
 
/*-------------------------------------------------------------------*/
inline
bool
operator!=( const double & lhs,
            const rcsc::AngleDeg & rhs )
{
    return std::fabs( lhs - rhs.degree() ) > rcsc::AngleDeg::EPSILON;
}
 
/*-------------------------------------------------------------------*/
inline
bool
operator==( const rcsc::AngleDeg & lhs,
            const rcsc::AngleDeg & rhs )
{
    return std::fabs( lhs.degree() - rhs.degree() ) < rcsc::AngleDeg::EPSILON;
}
 
/*-------------------------------------------------------------------*/
inline
bool
operator==( const rcsc::AngleDeg & lhs,
            const double & rhs )
{
    return std::fabs( lhs.degree() - rhs ) < rcsc::AngleDeg::EPSILON;
}
 
/*-------------------------------------------------------------------*/
inline
bool
operator==( const double & lhs,
            const rcsc::AngleDeg & rhs )
{
    return std::fabs( lhs - rhs.degree() ) < rcsc::AngleDeg::EPSILON;
}
 
 
/*-------------------------------------------------------------------*/
inline
std::ostream &
operator<<( std::ostream & os,
            const rcsc::AngleDeg & a )
{
    return a.print( os );
}
 
#endif