ClothoidList.hxx

/*--------------------------------------------------------------------------*\
 |                                                                          |
 |  Copyright (C) 2017                                                      |
 |                                                                          |
 |         , __                 , __                                        |
 |        /|/  \               /|/  \                                       |
 |         | __/ _   ,_         | __/ _   ,_                                |
 |         |   \|/  /  |  |   | |   \|/  /  |  |   |                        |
 |         |(__/|__/   |_/ \_/|/|(__/|__/   |_/ \_/|/                       |
 |                           /|                   /|                        |
 |                           \|                   \|                        |
 |                                                                          |
 |      Enrico Bertolazzi                                                   |
 |      Dipartimento di Ingegneria Industriale                              |
 |      Universita` degli Studi di Trento                                   |
 |      email: enrico.bertolazzi@unitn.it                                   |
 |                                                                          |
\*--------------------------------------------------------------------------*/
 
 
namespace G2lib {
 
  using std::vector;
 
  /*\
   |    ____ ____            _           ____
   |   / ___|___ \ ___  ___ | |_   _____|___ \ __ _ _ __ ___
   |  | |  _  __) / __|/ _ \| \ \ / / _ \ __) / _` | '__/ __|
   |  | |_| |/ __/\__ \ (_) | |\ V /  __// __/ (_| | | | (__
   |   \____|_____|___/\___/|_| \_/ \___|_____\__,_|_|  \___|
  \*/
 
 
  class G2solve2arc {
 
    real_type m_tolerance{real_type(1e-10)};
    integer   m_max_iter{20};
 
    real_type m_x0{real_type(0)};
    real_type m_y0{real_type(0)};
    real_type m_theta0{real_type(0)};
    real_type m_kappa0{real_type(0)};
 
    real_type m_x1{real_type(0)};
    real_type m_y1{real_type(0)};
    real_type m_theta1{real_type(0)};
    real_type m_kappa1{real_type(0)};
 
    // standard problem
    real_type m_lambda{real_type(0)};
    real_type m_phi{real_type(0)};
    real_type m_xbar{real_type(0)};
    real_type m_ybar{real_type(0)};
    real_type m_th0{real_type(0)};
    real_type m_th1{real_type(0)};
    real_type m_k0{real_type(0)};
    real_type m_k1{real_type(0)};
    real_type m_DeltaK{real_type(0)};
    real_type m_DeltaTheta{real_type(0)};
 
    ClothoidCurve m_S0{"G2solve2arc_S0"};
    ClothoidCurve m_S1{"G2solve2arc_S1"};
 
    void
    evalA(
      real_type   alpha,
      real_type   L,
      real_type & A
    ) const;
 
    void
    evalA(
      real_type   alpha,
      real_type   L,
      real_type & A,
      real_type & A_1,
      real_type & A_2
    ) const;
 
    void
    evalG(
      real_type alpha,
      real_type L,
      real_type th,
      real_type k,
      real_type G[2]
    ) const;
 
    void
    evalG(
      real_type alpha,
      real_type L,
      real_type th,
      real_type k,
      real_type G[2],
      real_type G_1[2],
      real_type G_2[2]
    ) const;
 
    void evalF( real_type const vars[2], real_type F[2] ) const;
 
    void
    evalFJ(
      real_type const vars[2],
      real_type       F[2],
      real_type       J[2][2]
    ) const;
 
    void build_solution( real_type alpha, real_type L );
 
  public:
 
    G2solve2arc() = default;
 
    ~G2solve2arc() = default;
 
    //void setup( GenericContainer const & gc );
 
    int
    build(
      real_type x0, real_type y0, real_type theta0, real_type kappa0,
      real_type x1, real_type y1, real_type theta1, real_type kappa1
    );
 
    void set_tolerance( real_type tol );
 
    void set_max_iter( integer miter );
 
    int solve();
 
    ClothoidCurve const & S0() const { return m_S0; }
 
    ClothoidCurve const & S1() const { return m_S1; }
 
    #ifdef CLOTHOIDS_BACK_COMPATIBILITY
    void setTolerance( real_type tol ) { set_tolerance( tol ); }
    void setMaxIter( integer miter ) { set_max_iter( miter ); }
    #endif
 
  };
 
  /*\
   |    ____ ____            _            ____ _     ____
   |   / ___|___ \ ___  ___ | |_   _____ / ___| |   / ___|
   |  | |  _  __) / __|/ _ \| \ \ / / _ \ |   | |  | |
   |  | |_| |/ __/\__ \ (_) | |\ V /  __/ |___| |__| |___
   |   \____|_____|___/\___/|_| \_/ \___|\____|_____\____|
  \*/
 
  class G2solveCLC {
 
    real_type m_tolerance{real_type(1e-10)};
    int       m_max_iter{20};
 
    real_type m_x0{real_type(0)};
    real_type m_y0{real_type(0)};
    real_type m_theta0{real_type(0)};
    real_type m_kappa0{real_type(0)};
    real_type m_x1{real_type(0)};
    real_type m_y1{real_type(0)};
    real_type m_theta1{real_type(0)};
    real_type m_kappa1{real_type(0)};
 
    // standard problem
    real_type m_lambda{real_type(0)};
    real_type m_phi{real_type(0)};
    real_type m_xbar{real_type(0)};
    real_type m_ybar{real_type(0)};
    real_type m_th0{real_type(0)};
    real_type m_th1{real_type(0)};
    real_type m_k0{real_type(0)};
    real_type m_k1{real_type(0)};
 
    ClothoidCurve m_S0{"G2solveCLC_S0"};
    ClothoidCurve m_SM{"G2solveCLC_SM"};
    ClothoidCurve m_S1{"G2solveCLC_S1"};
 
    bool build_solution( real_type sM, real_type thM );
 
  public:
 
    G2solveCLC() = default;
 
    ~G2solveCLC() = default;
 
    //void setup( GenericContainer const & gc );
 
    int
    build(
      real_type x0, real_type y0, real_type theta0, real_type kappa0,
      real_type x1, real_type y1, real_type theta1, real_type kappa1
    );
 
    void set_tolerance( real_type tol );
 
    void set_max_iter( integer miter );
 
    int solve();
 
    ClothoidCurve const & S0() const { return m_S0; }
 
    ClothoidCurve const & SM() const { return m_SM; }
 
    ClothoidCurve const & S1() const { return m_S1; }
 
    void save( ostream_type & stream ) const;
 
    #ifdef CLOTHOIDS_BACK_COMPATIBILITY
    void setTolerance( real_type tol ) { set_tolerance( tol ); }
    void setMaxIter( integer miter ) { set_max_iter( miter ); }
    #endif
 
  };
 
  /*\
   |    ____ ____            _           _____
   |   / ___|___ \ ___  ___ | |_   _____|___ /  __ _ _ __ ___
   |  | |  _  __) / __|/ _ \| \ \ / / _ \ |_ \ / _` | '__/ __|
   |  | |_| |/ __/\__ \ (_) | |\ V /  __/___) | (_| | | | (__
   |   \____|_____|___/\___/|_| \_/ \___|____/ \__,_|_|  \___|
  \*/
 
  class G2solve3arc {
 
    ClothoidCurve m_S0{"G2solve3arc_S0"};
    ClothoidCurve m_SM{"G2solve3arc_SM"};
    ClothoidCurve m_S1{"G2solve3arc_S1"};
 
    real_type m_tolerance{real_type(1e-10)};
    int       m_max_iter{100};
 
    // \f$ G^2 \f$ interpolation data
    real_type m_x0{real_type(0)};
    real_type m_y0{real_type(0)};
    real_type m_theta0{real_type(0)};
    real_type m_kappa0{real_type(0)};
    real_type m_x1{real_type(0)};
    real_type m_y1{real_type(0)};
    real_type m_theta1{real_type(0)};
    real_type m_kappa1{real_type(0)};
 
    // standard scaled problem
    real_type m_phi{real_type(0)};
    real_type m_Lscale{real_type(0)};
    real_type m_th0{real_type(0)};
    real_type m_th1{real_type(0)};
    real_type m_s0{real_type(0)};
    real_type m_s1{real_type(0)};
 
    // precomputed values
    real_type m_K0{real_type(0)},
              m_K1{real_type(0)},
              m_c0{real_type(0)},
              m_c1{real_type(0)},
              m_c2{real_type(0)},
              m_c3{real_type(0)},
              m_c4{real_type(0)},
              m_c5{real_type(0)},
              m_c6{real_type(0)},
              m_c7{real_type(0)},
              m_c8{real_type(0)},
              m_c9{real_type(0)},
              m_c10{real_type(0)},
              m_c11{real_type(0)},
              m_c12{real_type(0)},
              m_c13{real_type(0)},
              m_c14{real_type(0)};
 
    void
    evalFJ(
      real_type const vars[2],
      real_type       F[2],
      real_type       J[2][2]
    ) const;
 
    void evalF( real_type const vars[2], real_type F[2] ) const;
 
    void build_solution( real_type sM, real_type thM );
 
    int solve( real_type sM_guess, real_type thM_guess );
 
  public:
 
    G2solve3arc() = default;
 
    ~G2solve3arc() = default;
 
    //void setup( GenericContainer const & gc );
 
    void set_tolerance( real_type tol );
 
    void set_max_iter( integer miter );
 
    int
    build(
      real_type x0,
      real_type y0,
      real_type theta0,
      real_type kappa0,
      real_type x1,
      real_type y1,
      real_type theta1,
      real_type kappa1,
      real_type Dmax = 0,
      real_type dmax = 0
    );
 
    int
    build_fixed_length(
      real_type s0,
      real_type x0,
      real_type y0,
      real_type theta0,
      real_type kappa0,
      real_type s1,
      real_type x1,
      real_type y1,
      real_type theta1,
      real_type kappa1
    );
 
    ClothoidCurve const & S0() const { return m_S0; }
 
    ClothoidCurve const & S1() const { return m_S1; }
 
    ClothoidCurve const & SM() const { return m_SM; }
 
    real_type
    total_length() const {
      return m_S0.length() +
             m_S1.length() +
             m_SM.length();
    }
 
 
    real_type
    theta_total_variation() const {
      return m_S0.theta_total_variation() +
             m_S1.theta_total_variation() +
             m_SM.theta_total_variation();
    }
 
    real_type
    curvature_total_variation() const {
      return m_S0.curvature_total_variation() +
             m_S1.curvature_total_variation() +
             m_SM.curvature_total_variation();
    }
 
    real_type
    integral_curvature2() const {
      return m_S0.integral_curvature2() +
             m_S1.integral_curvature2() +
             m_SM.integral_curvature2();
    }
 
    real_type
    integral_jerk2() const {
      return m_S0.integral_jerk2() +
             m_S1.integral_jerk2() +
             m_SM.integral_jerk2();
    }
 
    real_type
    integral_snap2() const {
      return m_S0.integral_snap2() +
             m_S1.integral_snap2() +
             m_SM.integral_snap2();
    }
 
    real_type
    theta_min_max( real_type & thMin, real_type & thMax ) const;
 
    real_type
    delta_theta() const
    { real_type thMin, thMax; return theta_min_max( thMin, thMax ); }
 
    real_type curvature_min_max( real_type & kMin, real_type & kMax ) const;
 
    real_type theta( real_type s ) const;
 
    real_type theta_D( real_type s ) const;
 
    real_type theta_DD( real_type s ) const;
 
    real_type theta_DDD( real_type s ) const;
 
    real_type X( real_type s ) const;
 
    real_type Y( real_type s ) const;
 
    real_type x_begin() const { return m_S0.x_begin(); }
 
    real_type y_begin() const { return m_S0.y_begin(); }
 
    real_type kappa_begin() const { return m_S0.kappa_begin(); }
 
    real_type theta_begin() const { return m_S0.theta_begin(); }
 
    real_type x_end()const { return m_S1.x_end(); }
 
    real_type y_end() const { return m_S1.y_end(); }
 
    real_type kappa_end() const { return m_S1.kappa_end(); }
 
    real_type theta_end() const { return m_S1.theta_end(); }
 
    void
    eval(
      real_type   s,
      real_type & theta,
      real_type & kappa,
      real_type & x,
      real_type & y
    ) const;
 
    void eval( real_type s, real_type & x, real_type & y ) const;
 
    void eval_D( real_type s, real_type & x_D, real_type & y_D ) const;
 
    void eval_DD( real_type s, real_type & x_DD, real_type & y_DD ) const;
 
    void eval_DDD( real_type s, real_type & x_DDD, real_type & y_DDD ) const;
 
    void eval_ISO( real_type s, real_type offs, real_type & x, real_type & y ) const;
 
    void eval_ISO_D( real_type s, real_type offs, real_type & x_D, real_type & y_D ) const;
 
    void eval_ISO_DD( real_type s, real_type offs, real_type & x_DD, real_type & y_DD ) const;
 
    void eval_ISO_DDD( real_type s, real_type offs, real_type & x_DDD, real_type & y_DDD ) const;
 
    void
    rotate( real_type angle, real_type cx, real_type cy ) {
      m_S0.rotate( angle, cx, cy );
      m_S1.rotate( angle, cx, cy );
      m_SM.rotate( angle, cx, cy );
    }
 
    void
    translate( real_type tx, real_type ty ){
      m_S0.translate( tx, ty );
      m_S1.translate( tx, ty );
      m_SM.translate( tx, ty );
    }
 
    void
    reverse() {
      ClothoidCurve tmp(m_S0); m_S1 = m_S0; m_S0 = tmp;
      m_S0.reverse();
      m_S1.reverse();
      m_SM.reverse();
    }
 
    friend
    ostream_type &
    operator << ( ostream_type & stream, ClothoidCurve const & c );
 
    void save( ostream_type & stream ) const;
 
    // BACK COMPATIBILITY
    #ifdef CLOTHOIDS_BACK_COMPATIBILITY
    void setTolerance( real_type tol ) { set_tolerance( tol ); }
    void setMaxIter( integer miter ) { set_max_iter( miter ); }
    real_type thetaTotalVariation() const { return theta_total_variation(); }
    real_type thetaMinMax( real_type & thMin, real_type & thMax ) const { return theta_min_max(thMin,thMax); }
    real_type totalLength() const { return total_length(); }
    real_type integralCurvature2() const { return integral_curvature2(); }
    real_type integralJerk2() const { return integral_jerk2(); }
    real_type integralSnap2() const { return integral_snap2(); }
    real_type deltaTheta() const { return delta_theta(); }
    #endif
 
  };
 
  /*\
   |   ____ _       _   _           _     _ _     _     _
   |  / ___| | ___ | |_| |__   ___ (_) __| | |   (_)___| |_
   | | |   | |/ _ \| __| '_ \ / _ \| |/ _` | |   | / __| __|
   | | |___| | (_) | |_| | | | (_) | | (_| | |___| \__ \ |_
   |  \____|_|\___/ \__|_| |_|\___/|_|\__,_|_____|_|___/\__|
   |
  \*/
  class ClothoidList : public BaseCurve {
 
    bool                  m_curve_is_closed{false};
    vector<real_type>     m_s0;
    vector<ClothoidCurve> m_clothoid_list;
 
    #ifdef CLOTHOIDS_USE_THREADS
    mutable Utils::BinarySearch<integer> m_last_interval;
    #else
    mutable integer m_last_interval{0};
    #endif
 
    mutable bool               m_aabb_done{false};
    mutable AABB_TREE          m_aabb_tree;
    mutable real_type          m_aabb_offs{real_type(0)};
    mutable real_type          m_aabb_max_angle{real_type(0)};
    mutable real_type          m_aabb_max_size{real_type(0)};
    mutable vector<Triangle2D> m_aabb_triangles;
 
    #ifdef CLOTHOIDS_USE_THREADS
    mutable std::mutex m_aabb_mutex;
    #endif
 
    void
    reset_last_interval() {
      #ifdef CLOTHOIDS_USE_THREADS
      bool ok;
      integer & last_interval = *m_last_interval.search( std::this_thread::get_id(), ok );
      #else
      integer & last_interval = m_last_interval;
      #endif
      last_interval = 0;
    }
 
    integer
    closest_point_internal(
      real_type   qx,
      real_type   qy,
      real_type   offs,
      real_type & x,
      real_type & y,
      real_type & s,
      real_type & DST
    ) const;
 
  public:
 
    #include "BaseCurve_using.hxx"
 
    ClothoidList() = delete;
 
    ClothoidList( string const & name ) : BaseCurve( name )
    { this->reset_last_interval(); }
 
    ~ClothoidList() override {
      m_s0.clear();
      m_clothoid_list.clear();
      m_aabb_triangles.clear();
    }
 
    void setup( GenericContainer const & gc ) override;
 
    ClothoidList( ClothoidList const & s ) : BaseCurve( s.name() )
    { this->reset_last_interval(); this->copy(s); }
 
    CurveType type() const override { return CurveType::CLOTHOID_LIST; }
 
    void init();
 
    void reserve( integer n );
 
    void copy( ClothoidList const & L );
 
    ClothoidList const & operator = ( ClothoidList const & s )
    { this->copy(s); return *this; }
 
    explicit ClothoidList( LineSegment const & LS );
 
    explicit ClothoidList( CircleArc const & C );
 
    explicit ClothoidList( Biarc const & B );
 
    explicit ClothoidList( BiarcList const & BL );
 
    explicit ClothoidList( ClothoidCurve const & CL );
 
    explicit ClothoidList( PolyLine const & PL );
 
    explicit ClothoidList( G2solve2arc const & C, string const & name );
 
    explicit ClothoidList( G2solve3arc const & C, string const & name  );
 
    explicit ClothoidList( G2solveCLC const & C, string const & name  );
 
    explicit ClothoidList( BaseCurve const * pC );
 
    void build( LineSegment const & );
    void build( CircleArc const & );
    void build( ClothoidCurve const & );
    void build( Biarc const & );
    void build( PolyLine const & );
    void build( BiarcList const & );
    void build( ClothoidList const & );
    void build( G2solve2arc const & );
    void build( G2solve3arc const & );
    void build( G2solveCLC const & );
    void build( Dubins const & );
    void build( Dubins3p const & );
 
    void push_back( LineSegment const & c );
 
    void push_back( CircleArc const & c );
 
    void push_back( Biarc const & c );
 
    void push_back( BiarcList const & c );
 
    void push_back( ClothoidCurve const & c );
 
    void push_back( ClothoidList const & c );
 
    void push_back( Dubins const & c );
 
    void push_back( Dubins3p const & c );
 
    void push_back( G2solve2arc const & c );
 
    void push_back( G2solve3arc const & c );
 
    void push_back( G2solveCLC const & c );
 
    void push_back( PolyLine const & c );
 
    void
    push_back(
      real_type kappa0,
      real_type dkappa,
      real_type L
    );
 
    void
    push_back(
      real_type x0,
      real_type y0,
      real_type theta0,
      real_type kappa0,
      real_type dkappa,
      real_type L
    );
 
    void
    push_back_G1(
      real_type x1,
      real_type y1,
      real_type theta1
    );
 
    void
    push_back_G1(
      real_type x0,
      real_type y0,
      real_type theta0,
      real_type x1,
      real_type y1,
      real_type theta1
    );
 
    bool is_closed() const { return m_curve_is_closed; }
 
    void make_closed() { m_curve_is_closed = true; }
 
    void make_open() { m_curve_is_closed = false; }
 
    real_type closure_gap_x() const { return this->x_end() - this->x_begin(); }
 
    real_type closure_gap_y() const { return this->y_end() - this->y_begin(); }
 
    real_type closure_gap_tx() const { return this->tx_end() - this->tx_begin(); }
 
    real_type closure_gap_ty() const { return this->ty_end() - this->ty_begin(); }
 
    bool
    closure_check( real_type tol_xy = 1e-6, real_type tol_tg = 1e-6 ) const {
      return std::abs(closure_gap_x())  < tol_xy &&
             std::abs(closure_gap_y())  < tol_xy &&
             std::abs(closure_gap_tx()) < tol_tg &&
             std::abs(closure_gap_ty()) < tol_tg;
    }
 
    bool
    build_G1(
      integer         n,
      real_type const x[],
      real_type const y[]
    );
 
    bool
    build_G1(
      integer         n,
      real_type const x[],
      real_type const y[],
      real_type const theta[]
    );
 
    bool
    build(
      real_type       x0,
      real_type       y0,
      real_type       theta0,
      integer         n,
      real_type const s[],
      real_type const kappa[]
    );
 
    bool
    build(
      real_type                 x0,
      real_type                 y0,
      real_type                 theta0,
      vector<real_type> const & s,
      vector<real_type> const & kappa
    ) {
      if ( s.size() != kappa.size() ) return false;
      return build(
        x0, y0, theta0,
        integer(s.size()),
        &s.front(), &kappa.front()
      );
    }
 
    bool
    build_raw(
      integer         n,
      real_type const x[],
      real_type const y[],
      real_type const abscissa[],
      real_type const theta[],
      real_type const kappa[]
    );
 
    bool
    build_raw(
      vector<real_type> const & x,
      vector<real_type> const & y,
      vector<real_type> const & abscissa,
      vector<real_type> const & theta,
      vector<real_type> const & kappa
    ) {
      integer n = integer(x.size());
      if ( n != integer(y.size())        ||
           n != integer(abscissa.size()) ||
           n != integer(theta.size())    ||
           n != integer(kappa.size()) ) return false;
      return build_raw(
        n, &x.front(), &y.front(),
        &abscissa.front(), &theta.front(), &kappa.front()
      );
    }
 
    ClothoidCurve const & get( integer idx ) const;
 
    ClothoidCurve const & get_at_s( real_type s ) const;
 
    integer num_segments() const { return integer(m_clothoid_list.size()); }
 
    void wrap_in_range( real_type & s ) const;
 
    integer find_at_s( real_type & s ) const;
 
    // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
 
    real_type length() const override;
    real_type length_ISO( real_type offs ) const override;
 
    real_type
    segment_length( integer nseg ) const;
 
    real_type
    segment_length_ISO( integer nseg, real_type offs ) const;
 
    real_type
    segment_length_SAE( integer nseg, real_type offs ) const
    { return segment_length_ISO( nseg, -offs ); }
 
    /*\
     |  _    _   _____    _                _
     | | |__| |_|_   _| _(_)__ _ _ _  __ _| |___
     | | '_ \ '_ \| || '_| / _` | ' \/ _` | / -_)
     | |_.__/_.__/|_||_| |_\__,_|_||_\__, |_\___|
     |                               |___/
    \*/
 
    void
    bb_triangles(
      vector<Triangle2D> & tvec,
      real_type            max_angle = Utils::m_pi/6, // 30 degree
      real_type            max_size  = 1e100,
      integer              icurve    = 0
    ) const override;
 
    void
    bb_triangles_ISO(
      real_type            offs,
      vector<Triangle2D> & tvec,
      real_type            max_angle = Utils::m_pi/6, // 30 degree
      real_type            max_size  = 1e100,
      integer              icurve    = 0
    ) const override;
 
    void
    bb_triangles_SAE(
      real_type            offs,
      vector<Triangle2D> & tvec,
      real_type            max_angle = Utils::m_pi/6, // 30 degree
      real_type            max_size  = 1e100,
      integer              icurve    = 0
    ) const override {
      this->bb_triangles_ISO( -offs, tvec, max_angle, max_size, icurve );
    }
 
    #ifndef DOXYGEN_SHOULD_SKIP_THIS
    void
    build_AABBtree_ISO(
      real_type offs,
      real_type max_angle = Utils::m_pi/6, // 30 degree
      real_type max_size  = 1e100
    ) const;
    #endif
 
    /*\
     |   _     _
     |  | |__ | |__   _____  __
     |  | '_ \| '_ \ / _ \ \/ /
     |  | |_) | |_) | (_) >  <
     |  |_.__/|_.__/ \___/_/\_\
    \*/
 
    void
    bbox(
      real_type & xmin,
      real_type & ymin,
      real_type & xmax,
      real_type & ymax
    ) const override {
      bbox_ISO( 0, xmin, ymin, xmax, ymax );
    }
 
    void
    bbox_ISO(
      real_type   offs,
      real_type & xmin,
      real_type & ymin,
      real_type & xmax,
      real_type & ymax
    ) const override;
 
    /*\
     |   ____             _          _______           _
     |  | __ )  ___  __ _(_)_ __    / / ____|_ __   __| |
     |  |  _ \ / _ \/ _` | | '_ \  / /|  _| | '_ \ / _` |
     |  | |_) |  __/ (_| | | | | |/ / | |___| | | | (_| |
     |  |____/ \___|\__, |_|_| |_/_/  |_____|_| |_|\__,_|
     |              |___/
    \*/
 
    real_type
    theta_begin() const override
    { return m_clothoid_list.front().theta_begin(); }
 
    real_type
    theta_end() const override
    { return m_clothoid_list.back().theta_end(); }
 
    real_type
    x_begin() const override
    { return m_clothoid_list.front().x_begin(); }
 
    real_type
    y_begin() const override
    { return m_clothoid_list.front().y_begin(); }
 
    real_type
    x_end() const override
    { return m_clothoid_list.back().x_end(); }
 
    real_type
    y_end() const override
    { return m_clothoid_list.back().y_end(); }
 
    real_type
    x_begin_ISO( real_type offs ) const override
    { return m_clothoid_list.front().x_begin_ISO( offs ); }
 
    real_type
    y_begin_ISO( real_type offs ) const override
    { return m_clothoid_list.front().y_begin_ISO( offs ); }
 
    real_type
    x_end_ISO( real_type offs ) const override
    { return m_clothoid_list.back().x_end_ISO( offs ); }
 
    real_type
    y_end_ISO( real_type offs ) const override
    { return m_clothoid_list.back().y_end_ISO( offs ); }
 
    real_type
    tx_begin() const override
    { return m_clothoid_list.front().tx_begin(); }
 
    real_type
    ty_begin() const override
    { return m_clothoid_list.front().ty_begin(); }
 
    real_type
    tx_end() const override
    { return m_clothoid_list.back().tx_end(); }
 
    real_type
    ty_end() const override
    { return m_clothoid_list.back().ty_end(); }
 
    real_type
    nx_begin_ISO() const override
    { return m_clothoid_list.front().nx_begin_ISO(); }
 
    real_type
    ny_begin_ISO() const override
    { return m_clothoid_list.front().ny_begin_ISO(); }
 
    real_type
    nx_end_ISO() const override
    { return m_clothoid_list.back().nx_end_ISO(); }
 
    real_type
    ny_end_ISO() const override
    { return m_clothoid_list.back().ny_end_ISO(); }
 
    /*\
     |  _   _          _
     | | |_| |__   ___| |_ __ _
     | | __| '_ \ / _ \ __/ _` |
     | | |_| | | |  __/ || (_| |
     |  \__|_| |_|\___|\__\__,_|
    \*/
 
    real_type theta    ( real_type s ) const override;
    real_type theta_D  ( real_type s ) const override;
    real_type theta_DD ( real_type s ) const override;
    real_type theta_DDD( real_type s ) const override;
 
    /*\
     |  _____                   _   _   _
     | |_   _|   __ _ _ __   __| | | \ | |
     |   | |    / _` | '_ \ / _` | |  \| |
     |   | |   | (_| | | | | (_| | | |\  |
     |   |_|    \__,_|_| |_|\__,_| |_| \_|
    \*/
 
    real_type tx    ( real_type s ) const override;
    real_type ty    ( real_type s ) const override;
    real_type tx_D  ( real_type s ) const override;
    real_type ty_D  ( real_type s ) const override;
    real_type tx_DD ( real_type s ) const override;
    real_type ty_DD ( real_type s ) const override;
    real_type tx_DDD( real_type s ) const override;
    real_type ty_DDD( real_type s ) const override;
 
    // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
 
    void
    tg(
      real_type   s,
      real_type & tg_x,
      real_type & tg_y
    ) const override;
 
    void
    tg_D(
      real_type   s,
      real_type & tg_x_D,
      real_type & tg_y_D
    ) const override;
 
    void
    tg_DD(
      real_type   s,
      real_type & tg_x_DD,
      real_type & tg_y_DD
    ) const override;
 
    void
    tg_DDD(
      real_type   s,
      real_type & tg_x_DDD,
      real_type & tg_y_DDD
    ) const override;
 
    // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
 
    void
    evaluate(
      real_type   s,
      real_type & th,
      real_type & k,
      real_type & x,
      real_type & y
    ) const override;
 
    // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
 
    void
    evaluate_ISO(
      real_type   s,
      real_type   offs,
      real_type & th,
      real_type & k,
      real_type & x,
      real_type & y
    ) const override;
 
    // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
 
    real_type X    ( real_type s ) const override;
    real_type Y    ( real_type s ) const override;
    real_type X_D  ( real_type s ) const override;
    real_type Y_D  ( real_type s ) const override;
    real_type X_DD ( real_type s ) const override;
    real_type Y_DD ( real_type s ) const override;
    real_type X_DDD( real_type s ) const override;
    real_type Y_DDD( real_type s ) const override;
 
    // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
 
    void
    eval(
      real_type   s,
      real_type & x,
      real_type & y
    ) const override;
 
    void
    eval_D(
      real_type   s,
      real_type & x_D,
      real_type & y_D
    ) const override;
 
    void
    eval_DD(
      real_type   s,
      real_type & x_DD,
      real_type & y_DD
    ) const override;
 
    void
    eval_DDD(
      real_type   s,
      real_type & x_DDD,
      real_type & y_DDD
    ) const override;
 
    /*\
     |         __  __          _
     |   ___  / _|/ _|___  ___| |_
     |  / _ \| |_| |_/ __|/ _ \ __|
     | | (_) |  _|  _\__ \  __/ |_
     |  \___/|_| |_| |___/\___|\__|
    \*/
 
    real_type X_ISO    ( real_type s, real_type offs ) const override;
    real_type Y_ISO    ( real_type s, real_type offs ) const override;
    real_type X_ISO_D  ( real_type s, real_type offs ) const override;
    real_type Y_ISO_D  ( real_type s, real_type offs ) const override;
    real_type X_ISO_DD ( real_type s, real_type offs ) const override;
    real_type Y_ISO_DD ( real_type s, real_type offs ) const override;
    real_type X_ISO_DDD( real_type s, real_type offs ) const override;
    real_type Y_ISO_DDD( real_type s, real_type offs ) const override;
 
    // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
 
    void
    eval_ISO(
      real_type   s,
      real_type   offs,
      real_type & x,
      real_type & y
    ) const override;
 
    void
    eval_ISO_D(
      real_type   s,
      real_type   offs,
      real_type & x_D,
      real_type & y_D
    ) const override;
 
    void
    eval_ISO_DD(
      real_type   s,
      real_type   offs,
      real_type & x_DD,
      real_type & y_DD
    ) const override;
 
    void
    eval_ISO_DDD(
      real_type   s,
      real_type   offs,
      real_type & x_DDD,
      real_type & y_DDD
    ) const override;
 
    /*\
     |  _                        __
     | | |_ _ __ __ _ _ __  ___ / _| ___  _ __ _ __ ___
     | | __| '__/ _` | '_ \/ __| |_ / _ \| '__| '_ ` _ \
     | | |_| | | (_| | | | \__ \  _| (_) | |  | | | | | |
     |  \__|_|  \__,_|_| |_|___/_|  \___/|_|  |_| |_| |_|
    \*/
 
    void translate( real_type tx, real_type ty ) override;
    void rotate( real_type angle, real_type cx, real_type cy ) override;
    void scale( real_type sc ) override;
    void reverse() override;
    void change_origin( real_type newx0, real_type newy0 ) override;
    void trim( real_type s_begin, real_type s_end ) override;
    void trim( real_type s_begin, real_type s_end, ClothoidList & newCL ) const;
 
    /*\
     |      _ _     _
     |   __| (_)___| |_ __ _ _ __   ___ ___
     |  / _` | / __| __/ _` | '_ \ / __/ _ \
     | | (_| | \__ \ || (_| | | | | (_|  __/
     |  \__,_|_|___/\__\__,_|_| |_|\___\___|
    \*/
 
    integer
    closest_point_ISO(
      real_type   qx,
      real_type   qy,
      real_type & x,
      real_type & y,
      real_type & s,
      real_type & t,
      real_type & dst
    ) const override;
 
    integer
    closest_point_ISO(
      real_type   qx,
      real_type   qy,
      real_type   offs,
      real_type & x,
      real_type & y,
      real_type & s,
      real_type & t,
      real_type & dst
    ) const override;
 
    real_type
    closest_point_by_sample(
      real_type   ds,
      real_type   qx,
      real_type   qy,
      real_type & X,
      real_type & Y,
      real_type & S
    ) const;
 
    /*\
     |      _ _     _
     |   __| (_)___| |_ __ _ _ __   ___ ___
     |  / _` | / __| __/ _` | '_ \ / __/ _ \
     | | (_| | \__ \ || (_| | | | | (_|  __/
     |  \__,_|_|___/\__\__,_|_| |_|\___\___|
    \*/
 
    integer
    closest_segment( real_type qx, real_type qy ) const;
 
    integer
    closest_point_in_range_ISO(
      real_type   qx,
      real_type   qy,
      integer     icurve_begin,
      integer     icurve_end,
      real_type & x,
      real_type & y,
      real_type & s,
      real_type & t,
      real_type & dst,
      integer   & icurve
    ) const;
 
    integer
    closest_point_in_range_SAE(
      real_type   qx,
      real_type   qy,
      integer     icurve_begin,
      integer     icurve_end,
      real_type & x,
      real_type & y,
      real_type & s,
      real_type & t,
      real_type & dst,
      integer   & icurve
    ) const {
      integer res = this->closest_point_in_range_ISO(
        qx, qy, icurve_begin, icurve_end, x, y, s, t, dst, icurve
      );
      t = -t;
      return res;
    }
 
    integer
    closest_point_in_s_range_ISO(
      real_type   qx,
      real_type   qy,
      real_type   s_begin,
      real_type   s_end,
      real_type & x,
      real_type & y,
      real_type & s,
      real_type & t,
      real_type & dst,
      integer   & icurve
    ) const;
 
    integer
    closest_point_in_s_range_SAE(
      real_type   qx,
      real_type   qy,
      real_type   s_begin,
      real_type   s_end,
      real_type & x,
      real_type & y,
      real_type & s,
      real_type & t,
      real_type & dst,
      integer   & icurve
    ) const {
      integer res = this->closest_point_in_s_range_ISO(
        qx, qy, s_begin, s_end, x, y, s, t, dst, icurve
      );
      t = -t;
      return res;
    }
 
    string info() const;
 
    void
    info( ostream_type & stream ) const override
    { stream << this->info(); }
 
    friend
    ostream_type &
    operator << ( ostream_type & stream, ClothoidList const & CL );
 
    void
    get_SK( real_type s[], real_type kappa[] ) const;
 
    void
    get_SK(
      vector<real_type> & s,
      vector<real_type> & kappa
    ) const {
      s.resize( m_clothoid_list.size()+1 );
      kappa.resize( m_clothoid_list.size()+1 );
      get_SK( &s.front(), &kappa.front() );
    }
 
    void
    get_STK(
      real_type s[],
      real_type theta[],
      real_type kappa[]
    ) const;
 
    void
    get_STK(
      vector<real_type> & s,
      vector<real_type> & theta,
      vector<real_type> & kappa
    ) const {
      s.resize( m_clothoid_list.size()+1 );
      theta.resize( m_clothoid_list.size()+1 );
      kappa.resize( m_clothoid_list.size()+1 );
      get_STK( &s.front(), &theta.front(), &kappa.front() );
    }
 
    void
    get_XY( real_type x[], real_type y[] ) const;
 
    void
    get_delta_theta( real_type delta_theta[] ) const;
 
    void
    get_delta_kappa( real_type deltaKappa[] ) const;
 
    integer
    findST1(
      real_type   x,
      real_type   y,
      real_type & s,
      real_type & t
    ) const;
 
    integer
    findST1(
      integer     ibegin,
      integer     iend,
      real_type   x,
      real_type   y,
      real_type & s,
      real_type & t
    ) const;
 
    /*\
     |             _ _ _     _
     |    ___ ___ | | (_)___(_) ___  _ __
     |   / __/ _ \| | | / __| |/ _ \| '_ \
     |  | (_| (_) | | | \__ \ | (_) | | | |
     |   \___\___/|_|_|_|___/_|\___/|_| |_|
    \*/
 
    bool
    collision( ClothoidList const & CL ) const {
      return collision_ISO( 0, CL, 0 );
    }
 
    bool
    collision_ISO(
      real_type            offs,
      ClothoidList const & CL,
      real_type            offs_C
    ) const;
 
    bool
    collision( BaseCurve const * pC ) const override;
 
    bool
    collision_ISO(
      real_type         offs,
      BaseCurve const * pC,
      real_type         offs_C
    ) const override;
 
    /*\
     |   _       _                          _
     |  (_)_ __ | |_ ___ _ __ ___  ___  ___| |_
     |  | | '_ \| __/ _ \ '__/ __|/ _ \/ __| __|
     |  | | | | | ||  __/ |  \__ \  __/ (__| |_
     |  |_|_| |_|\__\___|_|  |___/\___|\___|\__|
    \*/
 
    void
    intersect(
      ClothoidList const & CL,
      IntersectList      & ilist
    ) const {
      this->intersect_ISO( 0, CL, 0, ilist );
    }
 
    void
    intersect_ISO(
      real_type            offs,
      ClothoidList const & CL,
      real_type            offs_obj,
      IntersectList      & ilist
    ) const;
 
    void
    intersect(
      BaseCurve const * pC,
      IntersectList   & ilist
    ) const override;
 
    void
    intersect_ISO(
      real_type         offs,
      BaseCurve const * pC,
      real_type         offs_LS,
      IntersectList   & ilist
    ) const override;
 
    void
    export_table( ostream_type & stream ) const;
 
    void
    export_ruby( ostream_type & stream ) const;
 
    void save( ostream_type & stream ) const;
 
    void load( istream_type & stream, real_type epsi = 1e-8 );
 
#ifdef CLOTHOIDS_BACK_COMPATIBILITY
#include "ClothoidList_compatibility.hxx"
#endif
 
  };
 
  /*\
   |
   |    ___ _     _   _        _    _ ___      _ _           ___ ___
   |   / __| |___| |_| |_  ___(_)__| / __|_ __| (_)_ _  ___ / __|_  )
   |  | (__| / _ \  _| ' \/ _ \ / _` \__ \ '_ \ | | ' \/ -_) (_ |/ /
   |   \___|_\___/\__|_||_\___/_\__,_|___/ .__/_|_|_||_\___|\___/___|
   |                                     |_|
  \*/
 
  class ClothoidSplineG2 {
  public:
    using TargetType = enum class TargetType : integer
    { P1, P2, P3, P4, P5, P6, P7, P8, P9 };
 
    static
    inline
    string
    to_string( TargetType n ) {
      string res{""};
      switch ( n ) {
      case TargetType::P1: res = "P1"; break;
      case TargetType::P2: res = "P2"; break;
      case TargetType::P3: res = "P3"; break;
      case TargetType::P4: res = "P4"; break;
      case TargetType::P5: res = "P5"; break;
      case TargetType::P6: res = "P6"; break;
      case TargetType::P7: res = "P7"; break;
      case TargetType::P8: res = "P8"; break;
      case TargetType::P9: res = "P9"; break;
      }
      return res;
    };
 
  private:
 
    Utils::Malloc<real_type> real_values{"ClothoidSplineG2"};
 
    real_type * m_x{nullptr};
    real_type * m_y{nullptr};
    TargetType  m_tt{TargetType::P1};
    real_type   m_theta_I{real_type(0)};
    real_type   m_theta_F{real_type(0)};
    integer     m_npts{0};
 
    // work vector
    mutable real_type * m_k{nullptr};
    mutable real_type * m_dk{nullptr};
    mutable real_type * m_L{nullptr};
    mutable real_type * m_kL{nullptr};
    mutable real_type * m_L_1{nullptr};
    mutable real_type * m_L_2{nullptr};
    mutable real_type * m_k_1{nullptr};
    mutable real_type * m_k_2{nullptr};
    mutable real_type * m_dk_1{nullptr};
    mutable real_type * m_dk_2{nullptr};
 
    real_type
    diff2pi( real_type in ) const {
      return in-Utils::m_2pi*round(in/Utils::m_2pi);
    }
 
  public:
 
    ClothoidSplineG2() = default;
    ~ClothoidSplineG2() = default;
 
    void
    setP1( real_type theta0, real_type thetaN ) {
      m_tt      = TargetType::P1;
      m_theta_I = theta0;
      m_theta_F = thetaN;
    }
 
    void setP2() { m_tt = TargetType::P2; }
    void setP3() { m_tt = TargetType::P3; }
    void setP4() { m_tt = TargetType::P4; }
    void setP5() { m_tt = TargetType::P5; }
    void setP6() { m_tt = TargetType::P6; }
    void setP7() { m_tt = TargetType::P7; }
    void setP8() { m_tt = TargetType::P8; }
    void setP9() { m_tt = TargetType::P9; }
 
    void
    build(
      real_type const xvec[],
      real_type const yvec[],
      integer         npts
    );
 
    integer numPnts() const { return m_npts; }
    integer numTheta() const;
    integer numConstraints() const;
 
    void
    guess(
      real_type theta_guess[],
      real_type theta_min[],
      real_type theta_max[]
    ) const;
 
    bool
    objective( real_type const theta[], real_type & f ) const;
 
    bool
    gradient( real_type const theta[], real_type g[] ) const;
 
    bool
    constraints( real_type const theta[], real_type c[] ) const;
 
    integer
    jacobian_nnz() const;
 
    bool
    jacobian_pattern( integer i[], integer j[] ) const;
 
    bool
    jacobian_pattern_matlab( real_type i[], real_type j[] ) const;
 
    bool
    jacobian( real_type const theta[], real_type vals[] ) const;
 
    string info() const;
 
    void
    info( ostream_type & stream ) const
    { stream << this->info(); }
 
    friend
    ostream_type &
    operator << ( ostream_type & stream, ClothoidSplineG2 const & c );
 
  };
 
}