Added adjustment for speed

gregjesl · Jan 28, 2023 · 764056b · 764056b
1 parent 0da5ad2
commit 764056b
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 2 deletions.
diff --git a/inc/glnav_dijkstra.h b/inc/glnav_dijkstra.h
@@ -89,6 +89,13 @@ namespace glnav
                 {
                     if(result[i].first == next) throw std::runtime_error("Loop detected");
                 }
+
+                // Set the speed
+                const Q delta_cost = this->__map.cost(result.back().first) - this->__map.cost(next);
+                const Q distance = path<T>(result.back().first, next).length();
+                result.back().second = distance / delta_cost;
+
+                // Set the new waypoint
                 result.push_back(next, 1);
             }
 

diff --git a/inc/glnav_route.h b/inc/glnav_route.h
@@ -18,6 +18,7 @@ namespace glnav
             __seed(seed)
         { }
 
+        using std::deque<std::pair<point<T>, Q> >::empty;
         using std::deque<std::pair<point<T>, Q> >::size;
         using std::deque<std::pair<point<T>, Q> >::clear;
         using std::deque<std::pair<point<T>, Q> >::operator[];
@@ -54,14 +55,28 @@ namespace glnav
             return std::deque<std::pair<point<T>, Q> >::pop_back();
         }
 
-        std::pair<point<T>, Q> front()
+        std::pair<point<T>, Q> & front()
         {
             assert_version(*this, this->__seed);
 
             return std::deque<std::pair<point<T>, Q> >::front();
         }
 
-        std::pair<point<T>, Q> back()
+        std::pair<point<T>, Q> & back()
+        {
+            assert_version(*this, this->__seed);
+
+            return std::deque<std::pair<point<T>, Q> >::back();
+        }
+
+        const std::pair<point<T>, Q> & front() const
+        {
+            assert_version(*this, this->__seed);
+
+            return std::deque<std::pair<point<T>, Q> >::front();
+        }
+
+        const std::pair<point<T>, Q> & back() const
         {
             assert_version(*this, this->__seed);