src/robomath/robomath.h

   1 /*
   2  * @(#)robomath.h       07/08/13
   3  *
   4  * Description  : useful matemathic functions.
   5  *
   6  * License      : GNU GPL v.2
   7  */
   8
   9 #ifndef __robomath__
  10 #define __robomath__
  11
  12 #include <math.h>
  13 #include <stdlib.h>
  14
  15 #define POWER(x)                ((x)*(x))
  16 #define DEG2RAD(a)              ((a)/180.0*M_PI)
  17 #define RAD2DEG(a)              ((a)*180.0/M_PI)
  18 #define DEGREES(a)              RAD2DEG(a)
  19 #define MIN(a, b)               (((a) < (b)) ? (a) : (b))
  20 #define MAX(a, b)               (((a) > (b)) ? (a) : (b))
  21
  22 /* used to analyze angles frequency */
  23 struct angles_freq {
  24         double angle;
  25         int frequency;
  26 };
  27
  28 #ifdef MATLAB_MEX_FILE
  29 #define inline
  30 #endif
  31
  32 /**
  33  * FUNCTION PROTOTYPES
  34  */
  35 #ifdef __cplusplus
  36 extern "C" {
  37 #endif
  38 static inline double distance(double x1, double y1, double x2, double y2)
  39 {
  40         double x = x2-x1, y = y2-y1;
  41         return sqrt(x*x + y*y);
  42 }
  43 /* Uniform random */
  44 static inline float urand()
  45 {
  46         return ((float)rand()/RAND_MAX);
  47 }
  48 double gaussrand();
  49 double gaussian_random(void);
  50 float exprand(float lambda);
  51 void angles_freq_sort(struct angles_freq *angles, int count);
  52
  53 /**
  54  * Gaussian probability density function.
  55  *
  56  * @param  val          value we are looking for
  57  * @param  sigma        standard deviation
  58  */
  59 static inline double evaluate_gaussian(double val, double sigma)
  60 {
  61         return (1.0/(sqrt(2.0*M_PI) * sigma) *
  62                 exp(-0.5 * (val*val / (sigma*sigma))));
  63 }
  64
  65 /**
  66  * Evaluate two angles using gaussian.
  67  *
  68  * @param  a1           angle 1 (rad)
  69  * @param  a2           angle 2 (rad)
  70  * @param  sigma        standard deviation
  71  */
  72 static inline double evaluate_angles(double a1, double a2, double sigma)
  73 {
  74         double diff;
  75
  76         /* normalize angles */
  77 /*      a1 = fmod(a1, 2*M_PI); */
  78 /*      a2 = fmod(a2, 2*M_PI); */
  79
  80         diff = fabs(a1 - a2);
  81         if (diff > M_PI) diff = 2*M_PI-diff;
  82
  83         return evaluate_gaussian(diff, sigma);
  84 }
  85
  86 int cmpi(const void *a, const void *b);
  87 int cmpd(const void *a, const void *b);
  88 #ifdef __cplusplus
  89 }
  90 #endif
  91
  92 #endif  /* __robomath__ */