common.h

Particle filtering implementation and application to people tracking.

/** 
* 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*/
#ifndef PII
#define PII
const double pi = 3.1415926535897932384626433832795;
#endif
/** 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 drandom() {return double(rand()) / RAND_MAX;}
//LM changed random->random (conflict with stdlib)


/**
* 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)(drandom()*n*0.999999);}


/**
* draw number from normal random distribution
* @param mean the mean value
* @param std the standard deviation
* @return the random number
*/
inline double normaldist(const double &mean, const double &std)
{
  static const double r_max=RAND_MAX+1;
  return std*sqrt(-2*log((rand()+1)/r_max))*sin(2*pi*rand()/r_max)+mean;
} 

#endif // __Math_Common_h__