diff --git a/LibCarla/source/carla/client/Waypoint.cpp b/LibCarla/source/carla/client/Waypoint.cpp
index 0a53722f0..819aeb5de 100644
--- a/LibCarla/source/carla/client/Waypoint.cpp
+++ b/LibCarla/source/carla/client/Waypoint.cpp
@@ -87,7 +87,11 @@ namespace client {
       remaining_length = current_s;
     }
     remaining_length -= std::numeric_limits<double>::epsilon();
-    result.emplace_back(result.back()->GetNext(remaining_length).front());
+    if(result.size()) {
+      result.emplace_back(result.back()->GetNext(remaining_length).front());
+    } else {
+      result.emplace_back(GetNext(remaining_length).front());
+    }
 
     return result;
   }
@@ -114,7 +118,11 @@ namespace client {
       remaining_length = current_s;
     }
     remaining_length -= std::numeric_limits<double>::epsilon();
-    result.emplace_back(result.back()->GetPrevious(remaining_length).front());
+    if(result.size()) {
+      result.emplace_back(result.back()->GetPrevious(remaining_length).front());
+    } else {
+      result.emplace_back(GetPrevious(remaining_length).front());
+    }
 
     return result;
   }
diff --git a/LibCarla/source/carla/road/Map.cpp b/LibCarla/source/carla/road/Map.cpp
index 88936aa95..50a35e9ba 100644
--- a/LibCarla/source/carla/road/Map.cpp
+++ b/LibCarla/source/carla/road/Map.cpp
@@ -185,7 +185,7 @@ namespace road {
       } else if (delta_s <= 0) {
         return result_start;
       } else {
-        return GetNext(result_start, distance_to_segment.first).front();
+        return GetNext(result_start, delta_s).front();
       }
     } else {
       double delta_s = distance_to_segment.first;
@@ -195,7 +195,7 @@ namespace road {
       } else if (delta_s <= 0) {
         return result_start;
       } else {
-        return GetNext(result_start, distance_to_segment.first).front();
+        return GetNext(result_start, delta_s).front();
       }
     }
   }
diff --git a/LibCarla/source/carla/road/MapBuilder.cpp b/LibCarla/source/carla/road/MapBuilder.cpp
index 808bdb151..ee6e2bed4 100644
--- a/LibCarla/source/carla/road/MapBuilder.cpp
+++ b/LibCarla/source/carla/road/MapBuilder.cpp
@@ -840,7 +840,10 @@ namespace road {
         for (int i = 0; i < number_intervals; ++i) {
           if (interval < std::numeric_limits<double>::epsilon())
             break;
-          next_wp = map.GetNext(next_wp, interval).back();
+          auto next = map.GetNext(next_wp, interval);
+          if(next.size()){
+            next_wp = next.back();
+          }
 
           location = map.ComputeTransform(next_wp).location;
           get_min_max(location);
diff --git a/LibCarla/source/carla/road/element/Geometry.cpp b/LibCarla/source/carla/road/element/Geometry.cpp
index a16fd1133..a290d1e8c 100644
--- a/LibCarla/source/carla/road/element/Geometry.cpp
+++ b/LibCarla/source/carla/road/element/Geometry.cpp
@@ -136,10 +136,11 @@ namespace element {
     double current_s = 0;
     double current_u = 0;
     double last_u = 0;
-    double last_v = 0;
+    double last_v = _poly.Evaluate(current_u);
     double last_s = 0;
-    RtreeValue last_val;
+    RtreeValue last_val{last_u, last_v, last_s, _poly.Tangent(current_u)};
     while (current_s < _length + delta_u) {
+      current_u += delta_u;
       double current_v = _poly.Evaluate(current_u);
       double du = current_u - last_u;
       double dv = current_v - last_v;
@@ -157,7 +158,6 @@ namespace element {
       last_s = current_s;
       last_val = current_val;
 
-      current_u += delta_u;
     }
   }
 
@@ -167,7 +167,6 @@ namespace element {
 
     auto &val1 = result.second.first;
     auto &val2 = result.second.second;
-
     double rate = (val2.s - dist) / (val2.s - val1.s);
     double u = rate * val1.u + (1.0 - rate) * val2.u;
     double v = rate * val1.v + (1.0 - rate) * val2.v;
@@ -195,11 +194,17 @@ namespace element {
     }
     double param_p = 0;
     double current_s = 0;
-    double last_u = 0;
-    double last_v = 0;
+    double last_u = _polyU.Evaluate(param_p);
+    double last_v = _polyV.Evaluate(param_p);
     double last_s = 0;
-    RtreeValue last_val;
+    RtreeValue last_val{
+        last_u,
+        last_v,
+        last_s,
+        _polyU.Tangent(param_p),
+        _polyV.Tangent(param_p) };
     for(size_t i = 0; i < number_intervals; ++i) {
+      param_p += delta_p;
       double current_u = _polyU.Evaluate(param_p);
       double current_v = _polyV.Evaluate(param_p);
       double du = current_u - last_u;
@@ -224,7 +229,6 @@ namespace element {
       last_s = current_s;
       last_val = current_val;
 
-      param_p += delta_p;
       if(current_s > _length){
         break;
       }