Fawkes API  Fawkes Development Version
angle.h
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  * @return distance between points
00060  */
00061 inline float
00062 distance(float x1, float y1, float x2, float y2)
00063 {
00064   return sqrt( (x2-x1) * (x2-x1) + (y2-y1) * (y2-y1) );
00065 }
00066 
00067 /** Normalize angle in radian between -PI (inclusive) and PI (exclusive).
00068  * The given angle in radians is taken as an angle on the unit circle.
00069  * It is then normalized into the range -PI and PI, such that it is the
00070  * exact same angle on the unit circle but in the usual angle range.
00071  * @param angle_rad original value
00072  * @return normalized angle
00073  */
00074 inline float 
00075 normalize_mirror_rad(float angle_rad)
00076 {
00077   const float pi = static_cast<float>(M_PI);
00078   if ( (angle_rad < -1.0f * pi) || (angle_rad >= pi) ) {
00079     return ( angle_rad - 2.0f * pi * round(angle_rad / (2.0f * pi)) );
00080   } else {
00081     return angle_rad;
00082   }
00083 }
00084 
00085 /** Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).
00086  * The given angle in radians is taken as an angle on the unit circle.
00087  * It is then normalized into the range 0 and 2*PI, such that it is the
00088  * exact same angle on the unit circle but in the usual angle range.
00089  * @param angle_rad original value
00090  * @return normalized angle
00091  */
00092 inline float
00093 normalize_rad(float angle_rad)
00094 {
00095   const float twopi = static_cast<float>(2 * M_PI);
00096   if ( (angle_rad < 0) || (angle_rad >= twopi) ) {
00097     return angle_rad - twopi * floor(angle_rad / twopi);
00098   } else {
00099     return angle_rad;
00100   }
00101 }
00102 
00103 
00104 /** Normalizes angle in radian between -3*PI and 3*PI.
00105  * If the angle is above 2*PI or below 2*PI the angle will be clipped.
00106  * The largest full amount of (-)2*PI is subtracted, such that only the amount
00107  * within the range [-2*PI, 2*PI] remains. Then (-)2*PI is added again.
00108  * @param angle_rad original value
00109  * @return normalized angle
00110  */
00111 inline float
00112 normalize_bigmirror_rad(float angle_rad)
00113 {
00114   if ( (angle_rad < -2*M_PI) || (angle_rad > 2*M_PI) ) {
00115     return (normalize_mirror_rad(angle_rad) + copysign(2*M_PI, angle_rad) );
00116   } else {
00117     return angle_rad;
00118   }
00119 }
00120 
00121 
00122 /** Determines the distance between two angle provided as radians. 
00123  * Quadrants of the angles are considered to determine really the minimal
00124  * angle difference.
00125  * @param angle_rad1 first angle in radian
00126  * @param angle_rad2 first angle in radian
00127  * @return distance between the two angles
00128  */
00129 inline float 
00130 angle_distance(float angle_rad1,
00131                float angle_rad2)
00132 {
00133   if(angle_rad2 > angle_rad1)
00134     {
00135       return angle_rad2 - angle_rad1 < M_PI ? angle_rad2 - angle_rad1 : - 2.0 * M_PI + angle_rad2 - angle_rad1; 
00136     }
00137   else
00138     {
00139       return angle_rad1 - angle_rad2 < M_PI ? angle_rad2 - angle_rad1 : 2.0 * M_PI - angle_rad1 + angle_rad2;   
00140     }
00141 }
00142 
00143 
00144 } // end namespace fawkes
00145 
00146 #endif