Implement MovementDistance

source/armarx/navigation/human/EuclideanDistance.h

@@ -33,7 +33,7 @@ namespace armarx::navigation::human
     class EuclideanDistance : public DistanceFunction
     {
     public:
-        EuclideanDistance(double min = 0);
+        EuclideanDistance();
         virtual double computeDistance(Human &h1, Human &h2);
     };
 }

source/armarx/navigation/human/MovementDistance.cpp

@@ -7,9 +7,9 @@ namespace armarx::navigation::human
 
     }
 
-    double armarx::navigation::human::MovementDistance::computeDistance(Human &h1, Human &h2)
+    double MovementDistance::computeDistance(Human &h1, Human &h2)
     {
-
+        return (h1.linearVelocity - h2.linearVelocity).norm();
     }
 }
 

source/armarx/navigation/human/MovementDistance.h

@@ -27,6 +27,10 @@
 
 namespace armarx::navigation::human
 {
+    /**
+     * @brief A DistanceFunction that defines the distance between two humans as the difference
+     * in velocity (that is, the norm of the linear velocity difference vector).
+     */
     class MovementDistance : public DistanceFunction
     {
     public: