00001
00002 /***************************************************************************
00003  *  angle.h - angle related math helper functions
00004  *
00005  *  Created: Wed Jul 13 16:51:46 2005 (from FireVision)
00006  *  Copyright  2005-2008  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version. A runtime exception applies to
00014  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00022  */
00023
00024 #ifndef __UTILS_MATH_ANGLE_H_
00025 #define __UTILS_MATH_ANGLE_H_
00026 
00027 #include <cmath>
00028
00029 namespace fawkes {
00030
00031 
00032 /** Convert an angle given in degrees to radians.
00033  * @param deg original value in degrees
00034  * @return converted value in radians
00035  */
00036 inline float
00037 deg2rad(float deg)
00038 {
00039   return (deg * M_PI / 180.f);
00040 }
00041
00042 
00043 /** Convert an angle given in radians to degrees.
00044  * @param rad original value in radians
00045  * @return converted value in degrees
00046  */
00047 inline float
00048 rad2deg(float rad)
00049 {
00050   return (rad * 180.f / M_PI);
00051 }
00052
00053 
00054 /** Get distance between two 2D cartesian coordinates.
00055  * @param x1 X coordinate of first point
00056  * @param y1 Y coordinate of first point
00057  * @param x2 X coordinate of second point
00058  * @param y2 Y coordinate of second point
00059  */
00060 inline float
00061 distance(float x1, float y1, float x2, float y2)
00062 {
00063   return sqrt( (x2-x1) * (x2-x1) + (y2-y1) * (y2-y1) );
00064 }
00065 
00066 /** Normalize angle in radian between -PI and PI.
00067  * The given angle in radians is taken as an angle on the unit circle.
00068  * It is then normalized into the range -PI and PI, such that it is the
00069  * exact same angle on the unit circle but in the usual angle range.
00070  * @param angle_rad original value
00071  * @return normalized angle
00072  */
00073 inline float
00074 normalize_mirror_rad(float angle_rad)
00075 {
00076   if ( (angle_rad < -M_PI) || (angle_rad > M_PI) ) {
00077     return ( angle_rad - 2 * M_PI * round(angle_rad / (2 * M_PI)) );
00078   } else {
00079     return angle_rad;
00080   }
00081 }
00082 
00083 /** Normalize angle in radian between 0 and 2*PI.
00084  * The given angle in radians is taken as an angle on the unit circle.
00085  * It is then normalized into the range 0 and 2*PI, such that it is the
00086  * exact same angle on the unit circle but in the usual angle range.
00087  * @param angle_rad original value
00088  * @return normalized angle
00089  */
00090 inline float
00091 normalize_rad(float angle_rad)
00092 {
00093   if ( (angle_rad < 0) || (angle_rad > 2 * M_PI) ) {
00094     return angle_rad - 2 * M_PI * floor(angle_rad / (M_PI * 2));
00095   } else {
00096     return angle_rad;
00097   }
00098 }
00099
00100 
00101 /** Normalizes angle in radian between -3*PI and 3*PI.
00102  * If the angle is above 2*PI or below 2*PI the angle will be clipped.
00103  * The largest full amount of (-)2*PI is subtracted, such that only the amount
00104  * within the range [-2*PI, 2*PI] remains. Then (-)2*PI is added again.
00105  * @param angle_rad original value
00106  * @return normalized angle
00107  */
00108 inline float
00109 normalize_bigmirror_rad(float angle_rad)
00110 {
00111   if ( (angle_rad < -2*M_PI) || (angle_rad > 2*M_PI) ) {
00112     return (normalize_mirror_rad(angle_rad) + copysign(2*M_PI, angle_rad) );
00113   } else {
00114     return angle_rad;
00115   }
00116 }
00117
00118 
00119 /** Determines the distance between two angle provided as radians. 
00120  * Quadrants of the angles are considered to determine really the minimal
00121  * angle difference.
00122  * @param angle_rad1 first angle in radian
00123  * @param angle_rad2 first angle in radian
00124  * @return distance between the two angles
00125  */
00126 inline float
00127 angle_distance(float angle_rad1,
00128                float angle_rad2)
00129 {
00130   if(angle_rad2 > angle_rad1)
00131     {
00132       return angle_rad2 - angle_rad1 < M_PI ? angle_rad2 - angle_rad1 : - 2.0 * M_PI + angle_rad2 - angle_rad1;
00133     }
00134   else
00135     {
00136       return angle_rad1 - angle_rad2 < M_PI ? angle_rad2 - angle_rad1 : 2.0 * M_PI - angle_rad1 + angle_rad2;
00137     }
00138 }
00139
00140
00141 } // end namespace fawkes
00142
00143 #endif