Social layers

source/armarx/navigation/human/test/human_test.cpp

@@ -40,6 +40,7 @@
 
 #include <armarx/navigation/Test.h>
 #include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/human/CombinedDistance.h>
 #include <armarx/navigation/human/MovementDistance.h>
 #include <armarx/navigation/human/types.h>
 
@@ -47,6 +48,7 @@ using armarx::navigation::core::Pose2D;
 using armarx::navigation::human::EuclideanDistance;
 using armarx::navigation::human::OrientationDistance;
 using armarx::navigation::human::MovementDistance;
+using armarx::navigation::human::CombinedDistance;
 using armarx::navigation::human::Human;
 
 BOOST_AUTO_TEST_CASE(testEuclideanDistance1_standardDistance)
@@ -137,6 +139,28 @@ BOOST_AUTO_TEST_CASE(testMovementDistance1_roughlyMovingTogether)
     BOOST_CHECK_CLOSE(d, 0.5, 0.001);
 }
 
+BOOST_AUTO_TEST_CASE(testCombinedDistance1_threeDistances)
+{
+    const CombinedDistance combined = CombinedDistance(2, 3);
+    Pose2D pose1 = Pose2D::Identity();
+    pose1.translation() = Eigen::Vector2f(0, 0);
+    pose1.linear() = Eigen::Rotation2Df(-M_PI_2).toRotationMatrix();
+    const Human h1 = {.pose = pose1,
+                      .linearVelocity = Eigen::Vector2f(1.0, 1.0),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    Pose2D pose2 = Pose2D::Identity();
+    pose2.translation() = Eigen::Vector2f(2, 0);
+    pose2.linear() = Eigen::Rotation2Df(M_PI_2).toRotationMatrix();
+    const Human h2 = {.pose = pose2,
+                      .linearVelocity = Eigen::Vector2f(1.3, 1.4),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    double d = combined.computeDistance(h1, h2);
+    printf("Distance is %.2f\n", d);
+    BOOST_CHECK_CLOSE(d, 2 * 1.5 * 2.5, 0.001);
+}
+
 //BOOST_AUTO_TEST_CASE(testPathLength)
 //{
 //    armarx::navigation::core::Path path{