robomath: Gaussian functions are made static inline

MCL performace: 22 ms -> 20 ms (on PC)

diff --git a/src/robomath/robomath.c b/src/robomath/robomath.c
index fe827cfe33776fa22e9381817046d60777f90714..d643e54402cce52c5c4b983cb5941f73da70e9ec 100644 (file)
--- a/src/robomath/robomath.c
+++ b/src/robomath/robomath.c
@@ -74,18 +74,6 @@ double gaussian_random(void)
        }
 }
 
-/**
- * Gaussian probability density function.
- *
- * @param  val         value we are looking for
- * @param  sigma       standard deviation
- */
-double evaluate_gaussian(double val, double sigma)
-{
-       return (1.0/(sqrt(2.0*M_PI) * sigma) *
-               exp(-0.5 * (val*val / (sigma*sigma))));
-}
-
 /* Random generator with exponential distribution */
 float exprand(float lambda)
 {
@@ -132,28 +120,6 @@ void angles_freq_sort(struct angles_freq *angles, int count)
        qsort(angles, count, sizeof(struct angles_freq), cmpangfreq);
 }
 
-/**
- * Evaluate two angles using gaussian.
- *
- * @param  a1          angle 1 (rad)
- * @param  a2          angle 2 (rad)
- * @param  sigma       standard deviation
- */
-inline double evaluate_angles(double a1, double a2, double sigma)
-{
-       double diff, diff1, diff2;
-
-       /* normalize angles */
-/*     a1 = fmod(a1, 2*M_PI); */
-/*     a2 = fmod(a1, 2*M_PI); */
-
-       diff1 = a1 - a2;
-       diff2 = 2.0*M_PI + a2 - a1;
-       diff = (diff1 < diff2) ? diff1 : diff2;
-       
-       return evaluate_gaussian(diff, sigma);
-}
-
 /**
  * Compare integers. Used by qsort().
  */

diff --git a/src/robomath/robomath.h b/src/robomath/robomath.h
index 1e98ddaafeaffc7c52e2448d8f69ecfedb39b4b9..48043a7ad216b84b822f1e6bf9185e9a2dc738dc 100644 (file)
--- a/src/robomath/robomath.h
+++ b/src/robomath/robomath.h
@@ -47,10 +47,43 @@ static inline double distance(double x1, double y1, double x2, double y2)
 double gaussrand();
 float urand();
 double gaussian_random(void);
-double evaluate_gaussian(double val, double sigma);
 float exprand(float lambda);
 void angles_freq_sort(struct angles_freq *angles, int count);
-inline double evaluate_angles(double a1, double a2, double sigma);
+
+/**
+ * Gaussian probability density function.
+ *
+ * @param  val         value we are looking for
+ * @param  sigma       standard deviation
+ */
+static inline double evaluate_gaussian(double val, double sigma)
+{
+       return (1.0/(sqrt(2.0*M_PI) * sigma) *
+               exp(-0.5 * (val*val / (sigma*sigma))));
+}
+
+/**
+ * Evaluate two angles using gaussian.
+ *
+ * @param  a1          angle 1 (rad)
+ * @param  a2          angle 2 (rad)
+ * @param  sigma       standard deviation
+ */
+static inline double evaluate_angles(double a1, double a2, double sigma)
+{
+       double diff, diff1, diff2;
+
+       /* normalize angles */
+/*     a1 = fmod(a1, 2*M_PI); */
+/*     a2 = fmod(a1, 2*M_PI); */
+
+       diff1 = a1 - a2;
+       diff2 = 2.0*M_PI + a2 - a1;
+       diff = (diff1 < diff2) ? diff1 : diff2;
+       
+       return evaluate_gaussian(diff, sigma);
+}
+
 int cmpi(const void *a, const void *b);
 int cmpd(const void *a, const void *b);
 #ifdef __cplusplus