INTRODUCTION Overview Download and Install Documentation Publications REPOSITORY Libraries DEVELOPER Dev Guide Dashboard PEOPLE Contributors Users Project Download Mailing lists

mathdefs.h /* * GearBox Project: Peer-Reviewed Open-Source Libraries for Robotics * http://gearbox.sf.net/ * Copyright (c) 2004-2008 Alex Brooks, Alexei Makarenko, Tobias Kaupp * * This distribution is licensed to you under the terms described in * the LICENSE file included in this distribution. * */ /*dox @file @brief Mathematical macros @ingroup orca_library_orcaiceutil */ #ifndef GBXUTILACFR_MATH_DEFINITIONS_H #define GBXUTILACFR_MATH_DEFINITIONS_H #include <assert.h> /***************************************************************************** * INCLUDE THE RELEVANT MATHS LIBRARIES *****************************************************************************/ // MSVC compiler requires this symbol before exposing the (apparently) // non-standard symbols M_PI, etc... #ifdef WIN32 #define _USE_MATH_DEFINES #endif #include <cmath> /***************************************************************************** * CONSTANTS *****************************************************************************/ // M_PI is not defined after including cmath for the MS visual studio compiler? #ifndef M_PI //dox Defines number Pi #define M_PI 3.14159265358979323846 #endif #ifndef NAN //dox Defines not-a-number #define NAN (__builtin_nanf("")) #endif #ifndef INF //dox Defines infinity #define INF (__builtin_inff()) #endif /***************************************************************************** * CONVERSION MACROS *****************************************************************************/ #ifndef DEG2RAD_RATIO //dox Convertion factor from degrees to radians. #define DEG2RAD_RATIO (M_PI/180.0) #endif //dox Converts from degrees to radians. #define DEG2RAD(deg) ((deg)*DEG2RAD_RATIO) //dox Converts from radians to degrees. #define RAD2DEG(rad) ((rad)/DEG2RAD_RATIO) //dox Normalises the angle [rad] to the range [-pi,pi) //dox Don't return the normalised angle, because it's easy to make the //dox mistake of doing: 'NORMALISE_ANGLE( myAngle )', ignoring the return value. inline void NORMALISE_ANGLE( double &theta ) { double multiplier; if (theta >= -M_PI && theta < M_PI) return; multiplier = std::floor(theta / (2*M_PI)); theta -= multiplier*2*M_PI; if (theta >= M_PI) theta -= 2*M_PI; else if (theta < -M_PI) theta += 2*M_PI; } //dox Normalises the angle [rad] to the range [-pi,pi) //dox Don't return the normalised angle, because it's easy to make the //dox mistake of doing: 'NORMALISE_ANGLE( myAngle )', ignoring the return value. inline void NORMALISE_ANGLE( float &theta ) { double thDouble = theta; NORMALISE_ANGLE( thDouble ); theta = (float)thDouble; } /***************************************************************************** * MATH MACROS *****************************************************************************/ // for compatability retain this old one //#ifndef ABS //#define ABS(x) (std::abs(x)) //#endif #ifndef MIN //dox Minimum of two numbers #define MIN(x, y) (((x) < (y)) ? (x) : (y)) #endif #ifndef MAX //dox Maximum of two numbers #define MAX(x, y) (((x) > (y)) ? (x) : (y)) #endif #ifndef APPLY_LIMITS //dox Limits x to an interval between min_x and max_x. //dox x must be a variable which can be assigned a value; //dox Compares using less_than and greater_than. #define APPLY_LIMITS(max_x, x, min_x) \ if((x)>(max_x)) x=(max_x); if((x)<(min_x)) x=(min_x); #endif #ifndef NORM2 //dox Norm of a 2D vector #define NORM2(x, y) (sqrt((x)*(x)+(y)*(y))) #endif #ifndef NORM3 //dox Norm of a 3D vector #define NORM3(x, y, z) (sqrt((x)*(x)+(y)*(y)+(z)*(z))) #endif #ifndef ROUND //dox Rounds double or float to the nearest integer. #define ROUND(x) ((int)(x+0.5)) #endif #ifndef ROUND_TO //dox Rounds @c n to the nearest whole number of multiples of @c d. //dox For example, ROUND_TO(8,5) and ROUND_TO(12,5) will both result in 10. #define ROUND_TO(n,d) (d*rint(n/d)) #endif #ifndef SIGN //dox Sign of a number. #define SIGN(A) ((A)<0?(-1):(1)) #endif #ifndef COS_LAW //dox Law of cosines. #define COS_LAW(side1, side2, theta) \ (sqrt(SQR(side1)+SQR(side2)-2.0*(side1)*(side2)*cos(theta))) #endif #ifndef INV_COS_LAW //dox Inverse law of cosines.. #define INV_COS_LAW(oppSide, side1, side2) \ (acos((SQR(side1)+SQR(side2)-SQR(oppSide))/(2.0*(side1)*(side2)))) #endif // isinf not defined on all systems (eg Solaris) #if defined (__SVR4) && defined (__sun) #define isfinite(x) \ __extension__ ({ __typeof (x) __x_f = (x); \ __builtin_expect(!isnan(__x_f - __x_f), 1); }) #define isinf(x) \ __extension__ ({ __typeof (x) __x_i = (x); \ __builtin_expect(!isnan(__x_i) && !isfinite(__x_i), 0); }) #endif /***************************************************************************** * COMPARISON MACROS *****************************************************************************/ #ifndef NEAR //dox Check that two numbers are sufficiently close. //dox Compares using less_than and greater_than. #define NEAR(x,y,epsilon) (((x) > (y)-(epsilon)) && ((x) < (y)+(epsilon))) #endif //dox Modifies x to lie within [x_min,x_max] //dox template<typename T> void CLIP_TO_LIMITS( const T &min_x, T &x, const T &max_x ) { assert( min_x <= max_x ); if ( x > max_x ) x = max_x; else if ( x < min_x ) x = min_x; } #endif

---