Implement combined distance

source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.cpp

@@ -2,14 +2,15 @@
 
 namespace armarx::navigation::components::dynamic_scene_provider
 {
-    CombinedDistance::CombinedDistance()
+    CombinedDistance::CombinedDistance(double maxOrientationInfluence)
     {
-
+        this->euclidean = EuclideanDistance();
+        this->orientation = OrientationDistance(maxOrientationInfluence, 1);
     }
 
     double CombinedDistance::computeDistance(Human &h1, Human &h2)
     {
-
+        return orientation.computeDistance(h1, h2) * euclidean.computeDistance(h1, h2);
     }
 }
 

source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.h

 #pragma once
 
 #include "DistanceFunction.h"
+#include "EuclideanDistance.h"
+#include "OrientationDistance.h"
 
 
 namespace armarx::navigation::components::dynamic_scene_provider
@@ -8,7 +10,12 @@ namespace armarx::navigation::components::dynamic_scene_provider
     class CombinedDistance : DistanceFunction
     {
     public:
-        CombinedDistance();
+        CombinedDistance(double maxOrientationInfluence);
         virtual double computeDistance(Human &h1, Human &h2);
+
+    private:
+        EuclideanDistance euclidean;
+        OrientationDistance orientation;
+        double maxOrientationInfluence;
     };
 }