angle_deg.h

説明を見る。
00001 // -*-c++-*-
00002 
00008 /*
00009  *Copyright:
00010 
00011  Copyright (C) Hidehisa Akiyama
00012 
00013  This code is free software; you can redistribute it and/or
00014  modify it under the terms of the GNU Lesser General Public
00015  License as published by the Free Software Foundation; either
00016  version 2.1 of the License, or (at your option) any later version.
00017 
00018  This library is distributed in the hope that it will be useful,
00019  but WITHOUT ANY WARRANTY; without even the implied warranty of
00020  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00021  Lesser General Public License for more details.
00022 
00023  You should have received a copy of the GNU Lesser General Public
00024  License along with this library; if not, write to the Free Software
00025  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00026 
00027  *EndCopyright:
00028  */
00029 
00031 
00032 #ifndef RCSC_GEOM_ANGLEDEG_H
00033 #define RCSC_GEOM_ANGLEDEG_H
00034 
00035 #include <functional>
00036 #include <iostream>
00037 #include <cmath>
00038 
00039 namespace rcsc {
00040 
00045 class AngleDeg {
00046     // : public boost::addable< AngleDeg >
00047     // , public boost::addable2< AngleDeg, double >
00048     // , public boost::addable2_left< double,  AngleDeg >
00049     // , public boost::subtractable< AngleDeg >
00050     // , public boost::subtractable2< AngleDeg, double >
00051     // , public boost::subtractable2_left< double, AngleDeg >
00052     // , public boost::multipliable2< AngleDeg, double >
00053     // , public boost::dividable2< AngleDeg, double >
00054 private:
00056     double M_degree;
00057 
00058 public:
00060     static const double EPSILON;
00061 
00063     static const double DEG2RAD;
00065     static const double RAD2DEG;
00066 
00068     AngleDeg()
00069         : M_degree( 0.0 )
00070       { }
00078     AngleDeg( const double & deg )
00079         : M_degree( deg )
00080       {
00081           normalize();
00082       }
00083 
00089     const
00090     AngleDeg & operator=( const double & deg )
00091       {
00092           M_degree = deg;
00093           return normalize();
00094       }
00095 
00096 private:
00101     const
00102     AngleDeg & normalize()
00103       {
00104           if ( M_degree < -360.0 || 360.0 < M_degree )
00105           {
00106               M_degree = std::fmod( M_degree, 360.0 );
00107           }
00108 
00109           if ( M_degree < -180.0 )
00110           {
00111               M_degree += 360.0;
00112           }
00113 
00114           if ( M_degree > 180.0 )
00115           {
00116               M_degree -= 360.0;
00117           }
00118 
00119           return *this;
00120       }
00121 
00122 public:
00123 
00128     const
00129     double & degree() const
00130       {
00131           return M_degree;
00132       }
00133 
00138     double abs() const
00139       {
00140           return std::fabs( degree() );
00141       }
00146     double radian() const
00147       {
00148           return degree() * DEG2RAD;
00149       }
00150 
00151     /*
00152       operator double() const
00153       {
00154       return degree();
00155       }
00156     */
00157 
00162     AngleDeg operator-() const
00163       {
00164           return AngleDeg( - degree() );
00165       }
00166 
00172     const
00173     AngleDeg & operator+=( const AngleDeg & angle )
00174       {
00175           M_degree += angle.degree();
00176           return normalize();
00177       }
00178 
00184     const
00185     AngleDeg & operator+=( const double & deg )
00186       {
00187           M_degree += deg;
00188           return normalize();
00189       }
00190 
00196     const
00197     AngleDeg & operator-=( const AngleDeg & angle )
00198       {
00199           M_degree -= angle.degree();
00200           return normalize();
00201       }
00202 
00208     const
00209     AngleDeg & operator-=( const double & deg )
00210       {
00211           M_degree -= deg;
00212           return normalize();
00213       }
00214 
00220     const
00221     AngleDeg & operator*=( const double & scalar )
00222       {
00223           M_degree *= scalar;
00224           return normalize();
00225       }
00226 
00232     const
00233     AngleDeg & operator/=( const double & scalar )
00234       {
00235           if ( std::fabs( scalar ) < EPSILON )
00236           {
00237               return * this;
00238           }
00239           M_degree /= scalar;
00240           return normalize();
00241       }
00242 
00244 
00249     bool isLeftOf( const AngleDeg & angle ) const
00250       {
00251           //return (*this - angle).degree() < 0.0;
00252           double diff = angle.degree() - this->degree();
00253           return ( ( 0.0 < diff && diff < 180.0 )
00254                    || diff < -180.0 );
00255       }
00256 
00261     bool isLeftEqualOf( const AngleDeg & angle ) const
00262       {
00263           //return (*this - angle).degree() <= 0.0;
00264           double diff = angle.degree() - this->degree();
00265           return ( ( 0.0 <= diff && diff < 180.0 )
00266                    || diff < -180.0 );
00267       }
00268 
00273     bool isRightOf( const AngleDeg & angle ) const
00274       {
00275           //return (*this - angle).degree() > 0.0;
00276           double diff = this->degree() - angle.degree();
00277           return ( ( 0.0 < diff && diff < 180.0 )
00278                    || diff < -180.0 );
00279       }
00280 
00285     bool isRightEqualOf( const AngleDeg & angle ) const
00286       {
00287           //return (*this - angle).degree() >= 0.0;
00288           double diff = this->degree() - angle.degree();
00289           return ( ( 0.0 <= diff && diff < 180.0 )
00290                    || diff < -180.0 );
00291       }
00292 
00297     double cos() const
00298       {
00299           return std::cos( degree() * DEG2RAD );
00300       }
00301 
00306     double sin() const
00307       {
00308           return std::sin( degree() * DEG2RAD );
00309       }
00310 
00315     double tan() const
00316       {
00317           return std::tan( degree() * DEG2RAD );
00318       }
00319 
00326     bool isWithin( const AngleDeg & left,
00327                    const AngleDeg & right ) const;
00328 
00335     void sinMinMax( const double & angle_err,
00336                     double * minsin,
00337                     double * maxsin ) const;
00338 
00345     void cosMinMax( const double & angle_err,
00346                     double * mincos,
00347                     double * maxcos ) const;
00348 
00350     // static utility method concerned with angle operation
00351 
00357     inline
00358     static
00359     double normalize_angle( double dir )
00360       {
00361           if ( dir < -360.0 || 360.0 < dir )
00362           {
00363               dir = std::fmod( dir, 360.0 );
00364           }
00365 
00366           if ( dir < -180.0 )
00367           {
00368               dir += 360.0;
00369           }
00370 
00371           if ( dir > 180.0 )
00372           {
00373               dir -= 360.0;
00374           }
00375 
00376           return dir;
00377       }
00378 
00384     inline
00385     static
00386     double rad2deg( const double & rad )
00387       {
00388           return rad * RAD2DEG;
00389       }
00390 
00396     inline
00397     static
00398     double deg2rad( const double & deg )
00399       {
00400           return deg * DEG2RAD;
00401       }
00402 
00408     inline
00409     static
00410     double cos_deg( const double & deg )
00411       {
00412           return std::cos( deg2rad( deg ) );
00413       }
00414 
00420     inline
00421     static
00422     double sin_deg( const double & deg )
00423       {
00424           return std::sin( deg2rad( deg ) );
00425       }
00426 
00432     inline
00433     static
00434     double tan_deg( const double & deg )
00435       {
00436           return std::tan( deg2rad( deg ) );
00437       }
00438 
00444     inline
00445     static
00446     double acos_deg( const double & cosine )
00447       {
00448           return ( cosine >= 1.0
00449                    ? 0.0
00450                    : ( cosine <= -1.0
00451                        ? 180.0
00452                        : rad2deg( std::acos( cosine ) ) ) );
00453       }
00454 
00460     inline
00461     static
00462     double asin_deg( const double & sine )
00463       {
00464           return ( sine >= 1.0
00465                    ? 90.0
00466                    : ( sine <= -1.0
00467                        ? -90.0
00468                        : rad2deg( std::asin( sine ) ) ) );
00469       }
00470 
00476     inline
00477     static
00478     double atan_deg( const double & tangent )
00479       {
00480           return rad2deg( std::atan( tangent ) );
00481       }
00482 
00489     inline
00490     static
00491     double atan2_deg( const double & y,
00492                       const double & x )
00493       {
00494           return ( ( x == 0.0 && y == 0.0 )
00495                    ? 0.0
00496                    : rad2deg( std::atan2( y, x ) ) );
00497       }
00498 
00507     static
00508     AngleDeg bisect( const AngleDeg & left,
00509                      const AngleDeg & right );
00510 
00512 
00518     std::ostream & print( std::ostream & os ) const
00519       {
00520           return os << degree();
00521       }
00522 
00529     std::ostream & printRound( std::ostream & os,
00530                                const double & step = 0.1 ) const
00531       {
00532           return os << rint( degree() / step ) * step;
00533       }
00534 
00536 
00540     class DegreeCmp
00541         : public std::binary_function< AngleDeg, AngleDeg, bool > {
00542     public:
00544         result_type operator()( const first_argument_type & lhs,
00545                                 const second_argument_type & rhs ) const
00546           {
00547               return lhs.degree() < rhs.degree();
00548           }
00549     };
00550 
00551 };
00552 
00553 
00555 
00560 class AngleIsWithin
00561     : public std::unary_function< AngleDeg, bool > {
00562     const AngleDeg M_left; 
00563     const AngleDeg M_right; 
00564 public:
00566     AngleIsWithin( const AngleDeg & left,
00567                    const AngleDeg & right )
00568         : M_left( left )
00569         , M_right( right )
00570       { }
00571 
00573     result_type operator()( const argument_type & angle ) const
00574       {
00575           return angle.isWithin( M_left, M_right );
00576       }
00577 };
00578 
00579 
00580 } // end of namespace
00581 
00582 
00584 // arith operators
00585 
00586 /*-------------------------------------------------------------------*/
00593 inline
00594 const
00595 rcsc::AngleDeg
00596 operator+( const rcsc::AngleDeg & lhs,
00597            const rcsc::AngleDeg & rhs )
00598 {
00599     return rcsc::AngleDeg( lhs ) += rhs;
00600 }
00601 
00602 /*-------------------------------------------------------------------*/
00609 inline
00610 const
00611 rcsc::AngleDeg
00612 operator+( const rcsc::AngleDeg & lhs,
00613            const double & rhs )
00614 {
00615     return rcsc::AngleDeg( lhs ) += rhs;
00616 }
00617 
00618 /*-------------------------------------------------------------------*/
00625 inline
00626 const
00627 rcsc::AngleDeg
00628 operator+( const double & lhs,
00629            const rcsc::AngleDeg & rhs )
00630 {
00631     return rcsc::AngleDeg( rhs ) += lhs;
00632 }
00633 
00634 /*-------------------------------------------------------------------*/
00641 inline
00642 const
00643 rcsc::AngleDeg
00644 operator-( const rcsc::AngleDeg & lhs,
00645            const rcsc::AngleDeg & rhs )
00646 {
00647     return rcsc::AngleDeg( lhs ) -= rhs;
00648 }
00649 
00650 /*-------------------------------------------------------------------*/
00657 inline
00658 const
00659 rcsc::AngleDeg
00660 operator-( const rcsc::AngleDeg & lhs,
00661            const double & rhs )
00662 {
00663     return rcsc::AngleDeg( lhs ) -= rhs;
00664 }
00665 
00666 /*-------------------------------------------------------------------*/
00673 inline
00674 const
00675 rcsc::AngleDeg
00676 operator-( const double & lhs,
00677            const rcsc::AngleDeg & rhs )
00678 {
00679     return rcsc::AngleDeg( lhs ) -= rhs;
00680 }
00681 
00683 // predicate operator
00684 
00685 /*-------------------------------------------------------------------*/
00692 inline
00693 bool
00694 operator!=( const rcsc::AngleDeg & lhs,
00695             const rcsc::AngleDeg & rhs )
00696 {
00697     return std::fabs( lhs.degree() - rhs.degree() ) > rcsc::AngleDeg::EPSILON;
00698 }
00699 
00700 /*-------------------------------------------------------------------*/
00707 inline
00708 bool
00709 operator!=( const rcsc::AngleDeg & lhs,
00710             const double & rhs )
00711 {
00712     return std::fabs( lhs.degree() - rhs ) > rcsc::AngleDeg::EPSILON;
00713 }
00714 
00715 /*-------------------------------------------------------------------*/
00722 inline
00723 bool
00724 operator!=( const double & lhs,
00725             const rcsc::AngleDeg & rhs )
00726 {
00727     return std::fabs( lhs - rhs.degree() ) > rcsc::AngleDeg::EPSILON;
00728 }
00729 
00730 /*-------------------------------------------------------------------*/
00737 inline
00738 bool
00739 operator==( const rcsc::AngleDeg & lhs,
00740             const rcsc::AngleDeg & rhs )
00741 {
00742     return std::fabs( lhs.degree() - rhs.degree() ) < rcsc::AngleDeg::EPSILON;
00743 }
00744 
00745 /*-------------------------------------------------------------------*/
00752 inline
00753 bool
00754 operator==( const rcsc::AngleDeg& lhs,
00755             const double& rhs )
00756 {
00757     return std::fabs( lhs.degree() - rhs ) < rcsc::AngleDeg::EPSILON;
00758 }
00759 
00760 /*-------------------------------------------------------------------*/
00767 inline
00768 bool
00769 operator==( const double & lhs,
00770             const rcsc::AngleDeg & rhs )
00771 {
00772     return std::fabs( lhs - rhs.degree() ) < rcsc::AngleDeg::EPSILON;
00773 }
00774 
00775 
00776 /*-------------------------------------------------------------------*/
00783 inline
00784 std::ostream &
00785 operator<<( std::ostream & os,
00786             const rcsc::AngleDeg & a )
00787 {
00788     return a.print( os );
00789 }
00790 
00791 #endif

librcscに対してThu May 1 15:41:20 2008に生成されました。  doxygen 1.5.0