Tools/Math/Common.h

Go to the documentation of this file.

/**
 * @file Math/Common.h
 *
 * This contains some often used mathematical definitions and functions.
 *
 * @author <a href="mailto:martin.kallnik@gmx.de">Martin Kallnik</a>
 * @author Max Risler
 */

/** 
* defines the maximum of a and b
*/
#ifdef max
#undef max
#endif
#define max(a,b)            (((a) > (b)) ? (a) : (b))

/** 
* defines the minimum of a and b
*/
#ifdef min
#undef min
#endif
#define min(a,b)            (((a) < (b)) ? (a) : (b))

#ifndef __Math_Common_h__
#define __Math_Common_h__

#include <math.h>
#include <stdlib.h>


/**
* defines the sign of a
*/
#ifndef sgn
#define sgn(a)   ( (a) < 0 ? -1 : ((a)==0) ? 0 : 1 )
#endif

/**
* defines the square of a value
*/
#ifndef sqr
#define sqr(a) ( (a) * (a) )
#endif

inline double sec(const double a){return 1/cos(a);}
  
inline double cosec(const double a){return 1/sin(a);}

/** @name constants for some often used angles */
///@{
/** constant for a half circle*/
const double pi = 3.1415926535897932384626433832795;
/** constant for a full circle*/
const double pi2 = 2.0*3.1415926535897932384626433832795;
/** constant for three quater circle*/
const double pi3_2 = 1.5*3.1415926535897932384626433832795;
/** constant for a quarter circle*/
const double pi_2 = 0.5*3.1415926535897932384626433832795;
/** constant for a 1 degree*/
const double pi_180 = 3.1415926535897932384626433832795/180;
/** constant for a 1/8 circle*/
const double pi_4 = 3.1415926535897932384626433832795*0.25;
/** constant for a 3/8 circle*/
const double pi3_4 = 3.1415926535897932384626433832795*0.75;
///@}

/** @name constant for bayesian filter */
///@{
/** constant for e*/
const double e = 2.71828182845902353602874713527;
///@}
/** 
 * Converts angle from rad to long in microrad.
 * Robot uses long microrad for joint angles.
 * \param angle coded in rad
 * \return angle coded in microrad
 */
inline long toMicroRad(double angle) {return long(angle*1000000.0);}

/**
 * Converts angle long in microrad to rad.
 * \param microrad angle coded in microrad
 * \return angle coded in rad
 */
inline double fromMicroRad(long microrad){return ((double)microrad)/1000000.0;}

/** 
 * Converts angle from rad to degrees.
 * \param angle code in rad
 * \return angle coded in degrees
 */
inline double toDegrees(double angle){return angle * 180.0 / pi;}

/** Converts angle from degrees to rad.
 * \param degrees angle coded in degrees
 * \return angle coded in rad
 */
inline double fromDegrees(double degrees){return degrees * pi_180;}

/** 
* reduce angle to [-pi..+pi]
* \param data angle coded in rad
* \return normalized angle coded in rad
*/
inline double normalize(double data)
{
  if (data <= pi && data >= -pi) return data;
  double ndata = data - ((int )(data / pi2))*pi2;
  if (ndata > pi)
  {
    ndata -= pi2; 
  }
  else if (ndata < -pi)
  {
    ndata += pi2;
  }
  return ndata;
}

/**
* The function returns a random number in the range of [0..1].
* @return The random number.
*/
inline double random() {return double(rand()) / RAND_MAX;}

/**
* The function returns a random integer number in the range of [0..n-1].
* @param n the number of possible return values (0 ... n-1)
* @return The random number.
*/
inline int random(int n) {return (int)(random()*n*0.999999);}


/** 
 * Round floating point value to nearest integer.
 *
 * \note Not tested (or even useful?!?) on MIPS platform!
 * \see http://www.agner.org/assem/
 */
/*inline int fast_round(float val)
{
  double temp = val + 6755399441055744.0;
  return (int)*((__int64*)&temp);
}*/


/**
 * Magical fast floor function for floats.
 *
 * \note Not tested (or even useful?!?) on MIPS platform!
 * \see http://www.agner.org/assem/
 */
/*inline int fast_floor(float val)
{
  double temp = val + 6755399441055744.0;
  float  diff = (float)(val - (int)*((__int64*)&temp));
  return (int)*((__int64*)&temp) - (*(int*)&diff < 0);
}*/


#endif // __Math_Common_h__

/*
* Change log :
* 
* $Log: Common.h,v $
* Revision 1.3  2004/06/15 08:49:11  thomas
* work around: comment out new fast-round-functions because does not compile
*
* Revision 1.2  2004/06/14 23:37:26  kindler
* - added fast floor() and round() functions.
*
* Revision 1.1.1.1  2004/05/22 17:37:02  cvsadm
* created new repository GT2004_WM
*
* Revision 1.3  2004/03/09 13:26:44  dueffert
* serious bug fixed: int random(int) always returned 0 due to integer overflow on mips, may have worked on x86
*
* Revision 1.2  2003/12/21 13:37:07  goehring
* Euler constant added
*
* Revision 1.1  2003/10/07 10:13:23  cvsadm
* Created GT2004 (M.J.)
*
* Revision 1.2  2003/08/06 10:59:33  dueffert
* int random(int) added
*
* Revision 1.1.1.1  2003/07/02 09:40:28  cvsadm
* created new repository for the competitions in Padova from the 
* tamara CVS (Tuesday 2:00 pm)
*
* removed unused solutions
*
* Revision 1.12  2003/03/28 17:17:14  risler
* min/max now get redefined with each include of Common.h
*
* Revision 1.11  2003/03/06 18:17:29  dueffert
* fixed max problem (Math/Common.h has to be included after iostream.h!?!)
*
* Revision 1.10  2003/02/21 16:34:16  dueffert
* common pi in own code, warnings removed, platform compatibility restored
*
* Revision 1.9  2003/01/11 21:58:30  roefer
* Comment corrected
*
* Revision 1.8  2002/12/12 22:09:08  roefer
* Random functions added
*
* Revision 1.7  2002/11/20 13:37:31  dueffert
* missing or wrong doxygen file corrected
*
* Revision 1.6  2002/11/19 15:43:03  dueffert
* doxygen comments corrected
*
* Revision 1.5  2002/11/12 23:00:47  dueffert
* started restore greenhills compatibility
*
* Revision 1.4  2002/09/23 14:14:02  risler
* no message
*
* Revision 1.3  2002/09/23 13:54:48  risler
* abs replaced by fabs/labs
*
* Revision 1.2  2002/09/22 18:40:52  risler
* added new math functions, removed GTMath library
*
* Revision 1.1  2002/09/22 13:10:50  risler
* new Math headers added
*
*
*/

---