Fawkes API
Fawkes Development Version
|
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