Added spiral and splines geometry classes.

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

@@ -11,8 +11,12 @@
 #include "carla/geom/Location.h"
 #include "carla/geom/Math.h"
 
+#include <boost/array.hpp>
+#include <boost/math/tools/rational.hpp>
+
 #include <cephes/fresnel.h>
 
+#include <algorithm>
 #include <cmath>
 #include <stdexcept>
 
@@ -71,15 +75,78 @@ namespace element {
     p.location.x += static_cast<float>(C * cos_a - S * sin_a);
     p.location.y += static_cast<float>(S * cos_a + C * sin_a);
     p.tangent += length * length;
-
     return p;
   }
 
   /// @todo
   std::pair<float, float> GeometrySpiral::DistanceTo(const geom::Location &) const {
-    throw_exception(std::runtime_error("not implemented"));
+    //Not analytic, discretize and find nearest point
+    //throw_exception(std::runtime_error("not implemented"));
+    return {_start_position.x,_start_position.y};
   }
 
+  DirectedPoint GeometryPoly3::PosFromDist(double dist) const{
+    double u = dist;
+    double v = _a + _b*u + _c*u*u + _d*u*u*u;
+
+    double cos_hdg = std::cos(_heading);
+    double sin_hdg = std::cos(_heading);
+
+    double x = u * cos_hdg - v * sin_hdg;
+    double y = u * sin_hdg + v * cos_hdg;
+
+    double tangentv = _b + 2 * _c * u + 3 * _d * u * u;
+    double theta = std::atan2(tangentv, 1.0);
+
+    DirectedPoint p(_start_position, _heading + theta);
+    p.location.x += static_cast<float>(x);
+    p.location.y += static_cast<float>(y);
+    return p;
+  }
+
+  std::pair<float, float> GeometryPoly3::DistanceTo(const geom::Location &/*p*/) const{
+    //No analytical expression (Newton-Raphson?/point search)
+    //throw_exception(std::runtime_error("not implemented"));
+    return {_start_position.x,_start_position.y};
+  }
+
+  DirectedPoint GeometryParamPoly3::PosFromDist(const double dist) const
+  {
+    //from map repo
+    double p = dist;
+    if(!_arcLength){
+      p = geom::Math::Clamp(dist / _length, -1.0, 1.0);
+    }
+
+    auto polyU = boost::array<double, 4>{{_aU, _bU, _cU, _dU}};
+    auto polyV = boost::array<double, 4>{{_aV, _bV, _cV, _dV}};
+
+    double u = boost::math::tools::evaluate_polynomial(polyU, p);
+    double v = boost::math::tools::evaluate_polynomial(polyV, p);
+
+    const double cos_t = std::cos(_heading);
+    const double sin_t = std::sin(_heading);
+
+    double x = u * cos_t - v * sin_t;
+    double y = u * sin_t + v * cos_t;
+
+    auto tangentPolyU = boost::array<double, 4>{{_bU, 2.0 * _cU, 3.0 * _dU, 0.0}};
+    auto tangentPolyV = boost::array<double, 4>{{_bV, 2.0 * _cV, 3.0 * _dV, 0.0}};
+
+    double tangentU = boost::math::tools::evaluate_polynomial(tangentPolyU, p);
+    double tangentV = boost::math::tools::evaluate_polynomial(tangentPolyV, p);
+    double theta = atan2(tangentV, tangentU);
+
+    DirectedPoint point(_start_position, _heading + theta);
+    point.location.x += static_cast<float>(x);
+    point.location.y += static_cast<float>(y);
+    return point;
+  }
+  std::pair<float, float> GeometryParamPoly3::DistanceTo(const geom::Location &) const {
+    //No analytical expression (Newton-Raphson?/point search)
+    //throw_exception(std::runtime_error("not implemented"));
+    return {_start_position.x,_start_position.y};
+  }
 } // namespace element
 } // namespace road
 } // namespace carla

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

@@ -16,7 +16,9 @@ namespace element {
   enum class GeometryType : unsigned int {
     LINE,
     ARC,
-    SPIRAL
+    SPIRAL,
+    POLY3,
+    POLY3PARAM
   };
 
   struct DirectedPoint {
@@ -190,6 +192,116 @@ namespace element {
     double _curve_end;
   };
 
+  class GeometryPoly3 final : public Geometry {
+  public:
+    GeometryPoly3(
+        double start_offset, 
+        double length, 
+        double heading, 
+        const geom::Location &start_pos, 
+        double a, 
+        double b, 
+        double c, 
+        double d)
+        : Geometry(GeometryType::POLY3, start_offset, length, heading, start_pos),
+        _a(a),
+        _b(b),
+        _c(c),
+        _d(d){}
+
+    double Geta() const {
+      return _a;
+    }
+    double Getb() const {
+      return _b;
+    }
+    double Getc() const {
+      return _c;
+    }
+    double Getd() const {
+      return _d;
+    }
+
+    DirectedPoint PosFromDist(double dist) const override;
+
+    std::pair<float, float> DistanceTo(const geom::Location &) const override;
+
+  private:
+    double _a;
+    double _b;
+    double _c;
+    double _d;
+  };
+
+  class GeometryParamPoly3 final : public Geometry {
+  public:
+
+    GeometryParamPoly3(
+        double start_offset, 
+        double length, 
+        double heading, 
+        const geom::Location &start_pos, 
+        double aU, 
+        double bU, 
+        double cU, 
+        double dU,
+        double aV, 
+        double bV, 
+        double cV, 
+        double dV,
+        bool arcLength)
+        : Geometry(GeometryType::POLY3PARAM, start_offset, length, heading, start_pos),
+        _aU(aU),
+        _bU(bU),
+        _cU(cU),
+        _dU(dU),
+        _aV(aV),
+        _bV(bV),
+        _cV(cV),
+        _dV(dV),
+        _arcLength(arcLength){}
+
+    double GetaU() const {
+      return _aU;
+    }
+    double GetbU() const {
+      return _bU;
+    }
+    double GetcU() const {
+      return _cU;
+    }
+    double GetdU() const {
+      return _dU;
+    }
+    double GetaV() const {
+      return _aV;
+    }
+    double GetbV() const {
+      return _bV;
+    }
+    double GetcV() const {
+      return _cV;
+    }
+    double GetdV() const {
+      return _dV;
+    }
+
+    DirectedPoint PosFromDist(double dist) const override;
+
+    std::pair<float, float> DistanceTo(const geom::Location &) const override;
+
+  private:
+    double _aU;
+    double _bU;
+    double _cU;
+    double _dU;
+    double _aV;
+    double _bV;
+    double _cV;
+    double _dV;
+    bool _arcLength;
+  };
+
 } // namespace element
 } // namespace road
 } // namespace carla