Rewritten/simplify the angles evaluation function. Now it should be better

and faster.

diff --git a/src/mcl/mcl.c b/src/mcl/mcl.c
index 4a03bde89651f4e1dd0a5e9ea78fa387f6b2d309..18e15f137f15b213ffe2c7a2e6e470d3da1f531d 100644 (file)
--- a/src/mcl/mcl.c
+++ b/src/mcl/mcl.c
@@ -159,7 +159,7 @@ void mcl_update2(struct mcl_model *mcl, void *data)
                                /* printf("part_ang=%g measured_ang=%g\n", 
                                                parts[i].theta[it], angles->val[im]); */
                                p1 = evaluate_angles(parts[i].theta[it],
-                                       angles->val[im], DEG2RAD(2.0), mcl->aeval_sigma);
+                                               angles->val[im], mcl->aeval_sigma);
                                /* printf("p1 = %f\n", p1); */
                                /* FIXME: p[it] is initialized to 1
                                 * and I guess p1 is always less or

diff --git a/src/robomath/robomath.c b/src/robomath/robomath.c
index 12b459bba721d5f64eda4e918cfb5188398753da..0b066b1723462a69fa26ef2458cbcafddbccfd64 100644 (file)
--- a/src/robomath/robomath.c
+++ b/src/robomath/robomath.c
@@ -137,35 +137,19 @@ void angles_freq_sort(struct angles_freq *angles, int count)
  *
  * @param  a1          angle 1 (rad)
  * @param  a2          angle 2 (rad)
- * @param  at          angles toleration (rad) (used to solve problem:
- *                     i.e. 358 deg is closer to 1 deg than 5 deg)
  * @param  sigma       standard deviation
  */
-double evaluate_angles(double a1, double a2, double at, double sigma)
+inline double evaluate_angles(double a1, double a2, double sigma)
 {
-       double eval1, eval2, eval, sgn;
-       /* Normalize angles */
+       double eval1, eval2;
+
+       /* normalize angles */
 /*     a1 = fmod(a1, 2*M_PI); */
 /*     a2 = fmod(a1, 2*M_PI); */
-       if (((a1 <= at) || ((2*M_PI-a1) <= at)) &&
-               ((a2 < at) || ((2*M_PI-a2) < at))) {
-               if (a2 < M_PI)
-                       sgn = 1;
-               else 
-                       sgn = -1;
-               /* FIXME: Gausian symetric and monotonic (for
-                * abs. value). Therefore the comparison can be done
-                * directly on the angles and we do not need to
-                * calculate gausian two times. */
-               eval1 = evaluate_gaussian(a1 - a2, sigma);
-               eval2 = evaluate_gaussian((a1 - (a2 + sgn*2*M_PI)), sigma);
-                       
-               eval = (eval1>eval2) ? eval1 : eval2;
-       } else {
-               eval = evaluate_gaussian(a1 - a2, sigma);
-       }
+       eval1 = evaluate_gaussian(a1 - a2, sigma);
+       eval2 = evaluate_gaussian(2*M_PI - fabs(a1 - a2), sigma);
 
-       return eval;
+       return ((eval1 > eval2) ? eval1 : eval2);
 }
 
 /**

diff --git a/src/robomath/robomath.h b/src/robomath/robomath.h
index 53ef51212ffc079391098e95f42f6c6c39c5bf14..1e98ddaafeaffc7c52e2448d8f69ecfedb39b4b9 100644 (file)
--- a/src/robomath/robomath.h
+++ b/src/robomath/robomath.h
@@ -50,7 +50,7 @@ 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);
-double evaluate_angles(double a1, double a2, double at, double sigma);
+inline double evaluate_angles(double a1, double a2, double sigma);
 int cmpi(const void *a, const void *b);
 int cmpd(const void *a, const void *b);
 #ifdef __cplusplus