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