extended trajectory test

source/RobotAPI/libraries/core/test/TrajectoryTest.cpp

@@ -133,7 +133,7 @@ BOOST_AUTO_TEST_CASE(TrajectoryPerformanceTest)
     int count = 1000;
     for (int i = 0; i < count; ++i)
     {
-        timestamps.push_back(rand());
+        timestamps.push_back(i);
     }
     TrajectoryPtr traj(new Trajectory());
     for (int var = 0; var < 8; ++var)
@@ -158,6 +158,31 @@ BOOST_AUTO_TEST_CASE(TrajectoryPerformanceTest)
     ARMARX_INFO_S << "Unmarshalled";
     traj->getLength();
     ARMARX_INFO_S << "Length calculated";
+    IceUtil::Time start = IceUtil::Time::now();
+    float dummyVar = 0;
+    int dims = traj->dim();
+    for (const Trajectory::TrajData& data : *traj)
+    {
+        for (int d = 0; d < dims; ++d)
+        {
+            dummyVar += data.getPosition(d);
+        }
+    }
+    IceUtil::Time end = IceUtil::Time::now();
+    ARMARX_INFO << "time per iteration in microseconds: " << (end - start).toMicroSecondsDouble() / traj->size();
+    int iterationCount = 0;
+    start = IceUtil::Time::now();
+    for (float t = 0; t < 1000; t += 0.32)
+    {
+        for (int d = 0; d < dims; ++d)
+        {
+            dummyVar += traj->getState(t, d);
+        }
+        iterationCount++;
+    }
+    end = IceUtil::Time::now();
+    ARMARX_INFO << "time per iteration with interpolation in microseconds: " << (end - start).toMicroSecondsDouble() / iterationCount;
+    ARMARX_INFO << VAROUT(dummyVar);
     BOOST_CHECK_EQUAL(traj->getState(0), traj2->getState(0));
 }