Social layers

source/armarx/navigation/human/OrientationDistance.cpp

 #include "OrientationDistance.h"
 
 #include <boost/math/constants/constants.hpp>
+#include <math.h>
 
 namespace armarx::navigation::human
 {
@@ -26,10 +27,15 @@ namespace armarx::navigation::human
         const double angleH1 = Eigen::Rotation2Dd(h1.pose.linear()).angle();
         const double angleH2 = Eigen::Rotation2Dd(h2.pose.linear()).angle();
 
+        printf("line orientation = %.2f\n", lineOrientation);
+        printf("angle h1 = %.2f, angle h2 = %.2f\n", angleH1, angleH2);
+
         // assuming the angles to be in the interval [0, 2pi]
         const double deviationFirst = std::abs(normOrientation(angleH1 - lineOrientation));
         const double deviationSecond = std::abs(normOrientation(angleH2 - (lineOrientation + 1)));
 
+        printf("deviation first = %.2f, second = %.2f\n", deviationFirst, deviationSecond);
+
         // ranges from 0 (looking directly at each other) to 1 (back to back)
         return (deviationFirst + deviationSecond) / 2;
     }
@@ -48,7 +54,7 @@ namespace armarx::navigation::human
     double
     OrientationDistance::normOrientation(const double orientation)
     {
-        double convertedOrientation = orientation / pi;
+        double convertedOrientation = orientation / M_PI;
         // brings orientation to [0, 2)
         convertedOrientation = std::fmod(((std::fmod(convertedOrientation, 2)) + 2), 2);
         // brings orientation to [-1, 1)