From 26978ea1b16c2fc7c7dd0a2ca5774bb547f7984f Mon Sep 17 00:00:00 2001
From: Emma Broman <emma.broman@liu.se>
Date: Fri, 30 Apr 2021 13:46:43 +0200
Subject: [PATCH] Make randering of path work again

---
 modules/autonavigation/autonavigationhandler.cpp | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/modules/autonavigation/autonavigationhandler.cpp b/modules/autonavigation/autonavigationhandler.cpp
index a6f4c1c877..b5d571c4a2 100644
--- a/modules/autonavigation/autonavigationhandler.cpp
+++ b/modules/autonavigation/autonavigationhandler.cpp
@@ -368,11 +368,12 @@ std::vector<glm::dvec3> AutoNavigationHandler::curvePositions(int nPerSegment) {
     const double du = 1.0 / nPerSegment;
 
     for (std::unique_ptr<PathSegment>& p : _pathSegments) {
+        const double length = p->pathLength();
         for (double u = 0.0; u < 1.0; u += du) {
-            glm::dvec3 position = p->interpolatedPose(u).position;
+            glm::dvec3 position = p->interpolatedPose(u * length).position;
             positions.push_back(position);
         }
-        positions.push_back(p->interpolatedPose(1.0).position);
+        positions.push_back(p->end().position());
     }
 
     return positions;
@@ -391,11 +392,12 @@ std::vector<glm::dquat> AutoNavigationHandler::curveOrientations(int nPerSegment
     const double du = 1.0 / nPerSegment;
 
     for (std::unique_ptr<PathSegment>& p : _pathSegments) {
+        const double length = p->pathLength();
         for (double u = 0.0; u <= 1.0; u += du) {
-            const glm::dquat orientation = p->interpolatedPose(u).rotation;
+            const glm::dquat orientation = p->interpolatedPose(u * length).rotation;
             orientations.push_back(orientation);
         }
-        orientations.push_back(p->interpolatedPose(1.0).rotation);
+        orientations.push_back(p->end().rotation());
     }
 
     return orientations;