Sorted includes

LibCarla/source/carla/road/Map.h

@@ -6,11 +6,12 @@
 
 #pragma once
 
+#include "carla/geom/Math.h"
 #include "carla/Memory.h"
 #include "carla/NonCopyable.h"
 #include "carla/Optional.h"
-#include "carla/road/MapData.h"
 #include "carla/road/element/Waypoint.h"
+#include "carla/road/MapData.h"
 
 namespace carla {
 namespace road {
@@ -33,7 +34,7 @@ namespace road {
     }
 
     Map(MapData m)
-        : _data(std::move(m)) {}
+      : _data(std::move(m)) {}
 
   private:
 

LibCarla/source/carla/road/MapBuilder.cpp

@@ -47,10 +47,5 @@ namespace road {
     return SharedPtr<Map>(new Map{std::move(_map_data)});
   }
 
-  bool MapBuilder::InterpretRoadFlow() {
-    // todo
-    return false;
-  }
-
 } // namespace road
 } // namespace carla

LibCarla/source/carla/road/MapBuilder.h

@@ -6,8 +6,8 @@
 
 #pragma once
 
-#include "carla/road/Map.h"
 #include "carla/geom/Location.h"
+#include "carla/road/Map.h"
 
 #include <map>
 
@@ -31,8 +31,6 @@ namespace road {
       return r;
     }
 
-    bool InterpretRoadFlow();
-
   private:
 
     MapData _map_data;

LibCarla/source/carla/road/element/Geometry.h

@@ -6,8 +6,8 @@
 
 #pragma once
 
-#include "carla/geom/Math.h"
 #include "carla/geom/Location.h"
+#include "carla/geom/Math.h"
 #include "carla/opendrive/logic/cephes/fresnel.h"
 
 #include <cmath>
@@ -44,6 +44,14 @@ namespace element {
       return d;
     }
 
+    void ApplyLateralOffset(double lateral_offset) {
+      /// @todo Z axis??
+      auto normal_x = std::sin(tangent);
+      auto normal_y = -std::cos(tangent);
+      location.x += lateral_offset * normal_x;
+      location.y += lateral_offset * normal_y;
+    }
+
     friend bool operator==(const DirectedPoint &lhs, const DirectedPoint &rhs) {
       return (lhs.location == rhs.location) && (lhs.tangent == rhs.tangent);
     }

LibCarla/source/carla/road/element/RoadSegment.h

@@ -6,9 +6,9 @@
 
 #pragma once
 
-#include "carla/road/element/Types.h"
-#include "carla/road/element/RoadInfo.h"
 #include "carla/geom/Location.h"
+#include "carla/road/element/RoadInfo.h"
+#include "carla/road/element/Types.h"
 
 #include <set>
 #include <memory>

LibCarla/source/carla/road/element/Waypoint.h

@@ -6,8 +6,8 @@
 
 #pragma once
 
-#include "carla/Memory.h"
 #include "carla/geom/Transform.h"
+#include "carla/Memory.h"
 #include "carla/road/element/RoadInfoList.h"
 #include "carla/road/element/Types.h"
 
@@ -21,8 +21,6 @@ namespace element {
   class Waypoint {
   public:
 
-    Waypoint() = default;
-
     ~Waypoint();
 
     const geom::Transform &GetTransform() const {
@@ -42,12 +40,18 @@ namespace element {
 
   private:
 
+    friend carla::road::Map;
+
+    Waypoint() = default;
+
     SharedPtr<Map> _map;
 
     geom::Transform _transform;
 
     id_type _road_id = 0;
 
+    int _lane_id = 0;
+
     double _dist = 0.0;
 
   };