Improved Waypoint interface

LibCarla/source/carla/geom/Location.h

@@ -15,7 +15,7 @@
 namespace carla {
 namespace geom {
 
-  class Location : private Vector3D {
+  class Location : public Vector3D {
   public:
 
     Location() = default;

LibCarla/source/carla/geom/Math.h

@@ -6,9 +6,10 @@
 
 #pragma once
 
-#include "carla/geom/Vector3D.h"
 #include "carla/Debug.h"
+#include "carla/geom/Vector3D.h"
 
+#include <utility>
 #include <cmath>
 
 namespace carla {
@@ -29,8 +30,19 @@ namespace geom {
       return 0.5 * pi();
     }
 
+    static constexpr auto to_radiants() {
+      return 180.0 / pi();
+    }
+
+    static double clamp(
+        const double &a,
+        const double &min,
+        const double &max) {
+      return std::min(std::max(a, min), max);
+    }
+
     static double clamp01(double a) {
-      return std::min(std::max(a, 0.0), 1.0);
+      return clamp(a, 0.0, 1.0);
     }
 
     template <typename T>
@@ -62,17 +74,25 @@ namespace geom {
       return std::sqrt(DistanceSquared2D(a, b));
     }
 
-    static double DistanceSegmentPoint(
+    /// Returns a pair containing:
+    /// - @a first:  distance from v to p' where p' = p projected on segment (w - v)
+    /// - @a second: euclidean distance from p to p'
+    ///   @param p point to calculate distance
+    ///   @param v first point of the segment
+    ///   @param w second point of the segment
+    static std::pair<double, double> DistSegmentPoint(
         const Vector3D &p,
         const Vector3D &v,
         const Vector3D &w) {
       const double l2 = DistanceSquared2D(v, w);
+      const double l = std::sqrt(l2);
       if (l2 == 0.0) {
-        return Distance2D(p, v);
+        return std::make_pair(0.0, Distance2D(v, p));
       }
-      const double t = clamp01(Dot2D(p - v, w - v) / l2);
+      const double dot_p_w = Dot2D(p - v, w - v);
+      const double t = clamp01(dot_p_w / l2);
       const Vector3D projection = v + t * (w - v);
-      return Distance2D(p, projection);
+      return std::make_pair(t * l, Distance2D(projection, p));
     }
 
     static Vector3D RotatePointOnOrigin2D(Vector3D p, double angle) {
@@ -81,7 +101,10 @@ namespace geom {
       return Vector3D(p.x * c - p.y * s, p.x * s + p.y * c, 0.0);
     }
 
-    static double DistanceArcPoint(
+    /// Returns a pair containing:
+    /// - @a first:  distance across the arc from start_pos to p' where p' = p projected on Arc
+    /// - @a second: euclidean distance from p to p'
+    static std::pair<double, double> DistArcPoint(
         const Vector3D &p,
         const Vector3D &start_pos,
         const double length,
@@ -99,7 +122,7 @@ namespace geom {
       // check if the point is in the center of the circle, so we know p
       // is in the same distance of every possible point in the arc
       if (rotated_p == circ_center) {
-        return radius;
+        return std::make_pair(0.0, radius);
       }
 
       // use the arc length to calculate the angle in the last point of it
@@ -107,7 +130,6 @@ namespace geom {
       const double circumf = 2 * pi() * radius;
       const double last_point_angle = (length / circumf) * pi_double();
 
-
       // move the point relative to the center of the circle and find
       // the angle between the point and the center of coords in rad
       const double angle = std::atan2(intersection.y - radius, intersection.x) + pi_half();
@@ -115,7 +137,9 @@ namespace geom {
       // see if the angle is between 0 and last_point_angle
       DEBUG_ASSERT(angle >= 0);
       if (angle <= last_point_angle) {
-        return Distance2D(intersection, rotated_p);
+        return std::make_pair(
+            angle * radius,
+            Distance2D(intersection, rotated_p));
       }
 
       // find the nearest point, start or end to intersection
@@ -124,7 +148,9 @@ namespace geom {
           (radius * std::cos(last_point_angle - pi_half())) + circ_center.x,
           (radius * std::sin(last_point_angle - pi_half())) + circ_center.y, 0.0);
       const double end_dist = Distance2D(end_pos, rotated_p);
-      return (start_dist < end_dist) ? start_dist : end_dist;
+      return (start_dist < end_dist) ?
+          std::make_pair(0.0, start_dist) :
+          std::make_pair(length, end_dist);
     }
 
     static bool PointInRectangle(

LibCarla/source/carla/road/Map.cpp

@@ -0,0 +1,32 @@
+// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#include "carla/geom/Math.h"
+#include "carla/road/Map.h"
+#include "carla/road/element/Types.h"
+
+#include <limits>
+
+
+namespace carla {
+namespace road {
+
+  using namespace element;
+
+  Waypoint Map::GetClosestWaypointOnRoad(const geom::Location &loc) const {
+    return Waypoint(shared_from_this(), loc);
+  }
+
+  Optional<Waypoint> Map::GetWaypoint(const geom::Location &) const {
+    return Optional<Waypoint>();
+  }
+
+  const MapData &Map::GetData() const {
+    return _data;
+  }
+
+} // namespace road
+} // namespace carla

LibCarla/source/carla/road/Map.h

@@ -6,7 +6,6 @@
 
 #pragma once
 
-#include "carla/geom/Math.h"
 #include "carla/Memory.h"
 #include "carla/NonCopyable.h"
 #include "carla/Optional.h"
@@ -19,23 +18,18 @@ namespace road {
   class Map
     : public EnableSharedFromThis<Map>,
       private MovableNonCopyable {
+
   public:
 
-    element::Waypoint GetClosestWaypointOnRoad(const geom::Location &) const {
-      return element::Waypoint();
-    }
-
-    Optional<element::Waypoint> GetWaypoint(const geom::Location &) const {
-      return Optional<element::Waypoint>();
-    }
-
-    const MapData &GetData() {
-      return _data;
-    }
-
     Map(MapData m)
       : _data(std::move(m)) {}
 
+    element::Waypoint GetClosestWaypointOnRoad(const geom::Location &) const;
+
+    Optional<element::Waypoint> GetWaypoint(const geom::Location &) const;
+
+    const MapData &GetData() const;
+
   private:
 
     MapData _data;

LibCarla/source/carla/road/MapData.h

@@ -13,6 +13,9 @@
 
 namespace carla {
 namespace road {
+namespace element {
+  class Waypoint;
+}
 
   class MapData
     : private MovableNonCopyable {
@@ -32,6 +35,7 @@ namespace road {
 
     friend class MapBuilder;
     friend class Map;
+    friend element::Waypoint;
 
     MapData() = default;
 

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

@@ -9,6 +9,7 @@
 #include "carla/geom/Location.h"
 #include "carla/geom/Math.h"
 #include "carla/opendrive/logic/cephes/fresnel.h"
+#include "carla/Debug.h"
 
 #include <cmath>
 
@@ -81,6 +82,8 @@ namespace element {
 
     virtual const DirectedPoint PosFromDist(double dist) const = 0;
 
+    virtual std::pair<double, double> DistanceTo(const geom::Location &p) const = 0;
+
   protected:
 
     Geometry(
@@ -125,6 +128,14 @@ namespace element {
       p.location.y += dist * std::sin(p.tangent);
       return p;
     }
+
+    std::pair<double, double> DistanceTo(const geom::Location &p) const override {
+      return geom::Math::DistSegmentPoint(
+          p,
+          _start_position,
+          PosFromDist(_length).location);
+    }
+
   };
 
   class GeometryArc : public Geometry {
@@ -153,6 +164,20 @@ namespace element {
       return p;
     }
 
+    std::pair<double, double> DistanceTo(const geom::Location &p) const override {
+      /*const Vector3D &p,
+        const Vector3D &start_pos,
+        const double length,
+        const double heading, // [radians]
+        const double curvature*/
+      return geom::Math::DistArcPoint(
+          p,
+          _start_position,
+          _length,
+          _heading,
+          _curvature);
+    }
+
     double GetCurvature() {
       return _curvature;
     }
@@ -207,6 +232,11 @@ namespace element {
       return p;
     }
 
+    std::pair<double, double> DistanceTo(const geom::Location &) const override {
+      DEBUG_ERROR;
+      return {0.0, 0.0};
+    }
+
   private:
 
     double _curve_start;

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

@@ -90,10 +90,10 @@ namespace element {
       return (int) _lanes.size();
     }
 
-    std::vector<int> getLanesIDs(which_lane_e whichLanes = which_lane_e::Both) {
+    std::vector<int> getLanesIDs(which_lane_e whichLanes = which_lane_e::Both) const {
       std::vector<int> lanes_id;
 
-      for (lane_t::iterator it = _lanes.begin(); it != _lanes.end(); ++it) {
+      for (lane_t::const_iterator it = _lanes.begin(); it != _lanes.end(); ++it) {
         switch (whichLanes) {
           case which_lane_e::Both: {
             lanes_id.push_back(it->first);
@@ -115,8 +115,8 @@ namespace element {
       return lanes_id;
     }
 
-    const LaneInfo *getLane(int id) {
-      lane_t::iterator it = _lanes.find(id);
+    const LaneInfo *getLane(int id) const {
+      lane_t::const_iterator it = _lanes.find(id);
       return it == _lanes.end() ? nullptr : &it->second;
     }
   };

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

@@ -10,8 +10,9 @@
 #include "carla/road/element/RoadInfo.h"
 #include "carla/road/element/Types.h"
 
-#include <set>
+#include <limits>
 #include <memory>
+#include <set>
 #include <vector>
 
 namespace carla {
@@ -132,6 +133,43 @@ namespace element {
       return DirectedPoint::Invalid();
     }
 
+    std::pair<double, double> GetNearestPoint(const geom::Location &loc) const {
+      double min = std::numeric_limits<double>::max();
+      std::pair<double, double> last = {0.0, 0.0};
+
+      decltype(_geom)::const_iterator nearest_geom;
+
+      for (auto g = _geom.begin(); g !=_geom.end(); ++g) {
+        auto d = (*g)->DistanceTo(loc);
+        if (d.second < min) {
+          last = d;
+          min = d.second;
+          nearest_geom = g;
+        }
+      }
+
+      for (auto g = _geom.begin(); g != nearest_geom; ++g) {
+        last.first += (*g)->GetLength();
+      }
+
+      return last;
+    }
+
+    int GetNearestLane(double, const geom::Location &) const {
+      /*
+      DirectedPoint dp = GetDirectedPointIn(dist);
+
+      double dist_dp_loc = geom::Math::Distance2D(dp.location, loc);
+
+      const RoadInfoLane *road_info_lane = GetInfo<RoadInfoLane>(dist);
+      for (auto &&lane_id : road_info_lane->getLanesIDs()) {
+        const LaneInfo *info = road_info_lane->getLane(lane_id);
+        // search for info width
+      }
+      */
+      return 0;
+    }
+
     const double &GetLength() const {
       return _length;
     }

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

@@ -12,6 +12,38 @@ namespace carla {
 namespace road {
 namespace element {
 
+  Waypoint::Waypoint() {}
+
+  Waypoint::Waypoint(SharedPtr<const Map> m, const geom::Location &loc)
+    : _map(m) {
+
+    _road_id = 0;
+    _dist = 0;
+
+    double nearest_dist = std::numeric_limits<double>::max();
+    for (auto &&r : _map->GetData()._elements) {
+      auto current_dist = r.second->GetNearestPoint(loc);
+      if (current_dist.second < nearest_dist) {
+        nearest_dist = current_dist.second;
+        _road_id = r.first;
+        _dist = current_dist.first;
+      }
+    }
+
+    assert(_dist <= _map->GetData().GetRoad(_road_id)->GetLength());
+
+    //
+    const geom::Location loc_in_geom =
+        _map->GetData().GetRoad(_road_id)->GetDirectedPointIn(_dist).location;
+    const double heading =
+         _map->GetData().GetRoad(_road_id)->GetDirectedPointIn(_dist).tangent;
+
+    const geom::Rotation rot(0.0, heading * geom::Math::to_radiants(), 0.0);
+    _transform = geom::Transform(loc_in_geom, rot);
+
+    _lane_id = _map->GetData().GetRoad(_road_id)->GetNearestLane(_dist, loc);
+  }
+
   RoadInfoList Waypoint::GetRoadInfo() const {
     return RoadInfoList(_map->GetData().GetRoad(_road_id)->GetInfos(_dist));
   }

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

@@ -31,6 +31,7 @@ namespace element {
       return _road_id;
     }
 
+
     std::vector<Waypoint> Next(double distance) const {
       (void) distance;
       return std::vector<Waypoint>();
@@ -42,9 +43,11 @@ namespace element {
 
     friend carla::road::Map;
 
-    Waypoint() = default;
+    Waypoint();
 
-    SharedPtr<Map> _map;
+    Waypoint(SharedPtr<const Map>, const geom::Location &);
+
+    SharedPtr<const Map> _map;
 
     geom::Transform _transform;
 

LibCarla/source/test/test_geom.cpp

@@ -27,7 +27,7 @@ TEST(geom, distance) {
 }
 
 TEST(geom, nearest_point_segment) {
-  const float pos[] = {
+  const float segment[] = {
      0,  0, 10,  0,
      2,  5, 10,  8,
     -6,  8,  8, -2,
@@ -61,14 +61,16 @@ TEST(geom, nearest_point_segment) {
     double min_dist = std::numeric_limits<double>::max();
     int id = -1;
     for (int j = 0; j < 40; j += 4) {
-      const double dist = Math::DistanceSegmentPoint(point[i],
-          {pos[j], pos[j + 1], 0}, {pos[j + 2], pos[j + 3], 0});
+      const double dist = Math::DistSegmentPoint(
+          point[i],
+          {segment[j + 0], segment[j + 1], 0},
+          {segment[j + 2], segment[j + 3], 0}).second;
       if (dist < min_dist) {
         min_dist = dist;
         id = j / 4;
       }
     }
-    ASSERT_EQ(id, results[i]) << "Fails point number " << i;
+    ASSERT_EQ(id, results[i]) << "Fails point number: " << i;
   }
 }
 
@@ -112,12 +114,12 @@ TEST(geom, point_in_rectangle) {
 }
 
 TEST(geom, nearest_point_arc) {
-  ASSERT_NEAR(Math::DistanceArcPoint(Vector3D(1,0,0),
-      Vector3D(0,0,0), 1.57, 0, 1), 0.414214, 0.01);
-  ASSERT_NEAR(Math::DistanceArcPoint(Vector3D(2,1,0),
-      Vector3D(0,0,0), 1.57, 0, 1), 1.0, 0.01);
-  ASSERT_NEAR(Math::DistanceArcPoint(Vector3D(0,1,0),
-      Vector3D(0,0,0), 1.57, 0, 1), 1.0, 0.01);
-  ASSERT_NEAR(Math::DistanceArcPoint(Vector3D(1,2,0),
-      Vector3D(0,0,0), 1.57, 0, 1), 1.0, 0.01);
+  ASSERT_NEAR(Math::DistArcPoint(Vector3D(1,0,0),
+      Vector3D(0,0,0), 1.57, 0, 1).second, 0.414214, 0.01);
+  ASSERT_NEAR(Math::DistArcPoint(Vector3D(2,1,0),
+      Vector3D(0,0,0), 1.57, 0, 1).second, 1.0, 0.01);
+  ASSERT_NEAR(Math::DistArcPoint(Vector3D(0,1,0),
+      Vector3D(0,0,0), 1.57, 0, 1).second, 1.0, 0.01);
+  ASSERT_NEAR(Math::DistArcPoint(Vector3D(1,2,0),
+      Vector3D(0,0,0), 1.57, 0, 1).second, 1.0, 0.01);
 }