Use MovementDistance in CombinedDistance

source/armarx/navigation/human/CombinedDistance.cpp

@@ -2,15 +2,21 @@
 
 namespace armarx::navigation::human
 {
-    CombinedDistance::CombinedDistance(double maxOrientationInfluence)
+    CombinedDistance::CombinedDistance(double maxOrientationInfluence, double movementInfluence)
+    : maxOrientationInfluence(maxOrientationInfluence), movementInfluence(movementInfluence)
     {
         this->euclidean = EuclideanDistance();
-        this->orientation = OrientationDistance(maxOrientationInfluence, 1);
+        this->orientation = OrientationDistance();
+        this->movement = MovementDistance();
     }
 
     double CombinedDistance::computeDistance(Human &h1, Human &h2)
     {
-        return orientation.computeDistance(h1, h2) * euclidean.computeDistance(h1, h2);
+        return euclidean.computeDistance(h1, h2)
+                // scales the euclidean distance by a factor in [1, maxOrientationInfluence]
+                * (1 + (maxOrientationInfluence - 1) * orientation.computeDistance(h1, h2))
+                // scales the euclidean distance by a factor in [1, inf) depending on influence
+                * (1 + movementInfluence * movement.computeDistance(h1, h2));
     }
 }
 

source/armarx/navigation/human/CombinedDistance.h

@@ -24,6 +24,7 @@
 
 #include "DistanceFunction.h"
 #include "EuclideanDistance.h"
+#include "MovementDistance.h"
 #include "OrientationDistance.h"
 
 
@@ -43,13 +44,17 @@ namespace armarx::navigation::human
          * @brief CombinedDistance Creates a new combined distance with the specified orientation scale factor
          * @param maxOrientationInfluence the euclidean distance between the humans will be scaled
          * by up to this factor if they face away from each other
+         * @param movementInfluence scales the difference in velocity of the humans before applying
+         * it to the total distance
          */
-        CombinedDistance(double maxOrientationInfluence);
+        CombinedDistance(double maxOrientationInfluence, double movementInfluence);
         virtual double computeDistance(Human &h1, Human &h2);
 
     private:
         EuclideanDistance euclidean;
         OrientationDistance orientation;
+        MovementDistance movement;
         double maxOrientationInfluence;
+        double movementInfluence;
     };
 }