00001
00002
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
00047
00048
00049
00050
00051
00052
00053
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
00153
00154
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
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
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
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
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
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 }
00581
00582
00584
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
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