# Program Listing for File ClothoidList.hxx

↰ Return to documentation for file (`Clothoids/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 tolerance;
        int_type  maxIter;

        real_type x0;
        real_type y0;
        real_type theta0;
        real_type kappa0;

        real_type x1;
        real_type y1;
        real_type theta1;
        real_type kappa1;

        // standard problem
        real_type lambda, phi, xbar, ybar;
        real_type th0, th1;
        real_type k0, k1;
        real_type DeltaK;
        real_type DeltaTheta;

        ClothoidCurve S0, 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
        buildSolution( real_type alpha, real_type L );

      public:

        G2solve2arc()
        : tolerance(1e-10)
        , maxIter(20)
        , x0(0)
        , y0(0)
        , theta0(0)
        , kappa0(0)
        , x1(0)
        , y1(0)
        , theta1(0)
        , kappa1(0)
        , lambda(0)
        , phi(0)
        , xbar(0)
        , ybar(0)
        , th0(0)
        , th1(0)
        , k0(0)
        , k1(0)
        {}

        ~G2solve2arc() {}

        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 setTolerance( real_type tol );

        void setMaxIter( int tol );

        int solve();

        ClothoidCurve const & getS0() const { return S0; }

        ClothoidCurve const & getS1() const { return S1; }

      };

      /*\
       |    ____ ____            _            ____ _     ____
       |   / ___|___ \ ___  ___ | |_   _____ / ___| |   / ___|
       |  | |  _  __) / __|/ _ \| \ \ / / _ \ |   | |  | |
       |  | |_| |/ __/\__ \ (_) | |\ V /  __/ |___| |__| |___
       |   \____|_____|___/\___/|_| \_/ \___|\____|_____\____|
      \*/

      class G2solveCLC {

        real_type tolerance;
        int       maxIter;

        real_type x0;
        real_type y0;
        real_type theta0;
        real_type kappa0;
        real_type x1;
        real_type y1;
        real_type theta1;
        real_type kappa1;

        // standard problem
        real_type lambda, phi, xbar, ybar;
        real_type th0, th1;
        real_type k0, k1;

        ClothoidCurve S0, SM, S1;

        bool
        buildSolution( real_type sM, real_type thM );

      public:

        G2solveCLC()
        : tolerance(1e-10)
        , maxIter(20)
        , x0(0)
        , y0(0)
        , theta0(0)
        , kappa0(0)
        , x1(0)
        , y1(0)
        , theta1(0)
        , kappa1(0)
        , lambda(0)
        , phi(0)
        , xbar(0)
        , ybar(0)
        , th0(0)
        , th1(0)
        , k0(0)
        , k1(0)
        {}

        ~G2solveCLC() {}

        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 setTolerance( real_type tol );

        void setMaxIter( int tol );

        int solve();

        ClothoidCurve const & getS0() const { return S0; }

        ClothoidCurve const & getSM() const { return SM; }

        ClothoidCurve const & getS1() const { return S1; }

        void save( ostream_type & stream ) const;
      };

      /*\
       |    ____ ____            _           _____
       |   / ___|___ \ ___  ___ | |_   _____|___ /  __ _ _ __ ___
       |  | |  _  __) / __|/ _ \| \ \ / / _ \ |_ \ / _` | '__/ __|
       |  | |_| |/ __/\__ \ (_) | |\ V /  __/___) | (_| | | | (__
       |   \____|_____|___/\___/|_| \_/ \___|____/ \__,_|_|  \___|
      \*/

      class G2solve3arc {

        ClothoidCurve S0, SM, S1;

        real_type tolerance;
        int       maxIter;

        // G2 interpolation data
        real_type x0;
        real_type y0;
        real_type theta0;
        real_type kappa0;
        real_type x1;
        real_type y1;
        real_type theta1;
        real_type kappa1;

        // standard scaled problem
        real_type phi, Lscale;
        real_type th0, th1;
        real_type s0, s1;

        // precomputed values
        real_type K0, K1, c0, c1, c2, c3, c4, c5, c6, c7, c8, c9, c10, c11, c12, c13, c14;

        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
        buildSolution( real_type sM, real_type thM );

        int
        solve( real_type sM_guess, real_type thM_guess );

      public:

        G2solve3arc()
        : tolerance(1e-10)
        , maxIter(100)
        {}

        ~G2solve3arc() {}

        void setTolerance( real_type tol );

        void setMaxIter( int 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 & getS0() const { return S0; }

        ClothoidCurve const & getS1() const { return S1; }

        ClothoidCurve const & getSM() const { return SM; }

        real_type
        totalLength() const {
          return S0.length() + S1.length() + SM.length();
        }

        real_type
        thetaTotalVariation() const {
          return S0.thetaTotalVariation() +
                 S1.thetaTotalVariation() +
                 SM.thetaTotalVariation();
        }

        real_type
        curvatureTotalVariation() const {
          return S0.curvatureTotalVariation() +
                 S1.curvatureTotalVariation() +
                 SM.curvatureTotalVariation();
        }

        real_type
        integralCurvature2() const {
          return S0.integralCurvature2() +
                 S1.integralCurvature2() +
                 SM.integralCurvature2();
        }

        real_type
        integralJerk2() const {
          return S0.integralJerk2() +
                 S1.integralJerk2() +
                 SM.integralJerk2();
        }

        real_type
        integralSnap2() const {
          return S0.integralSnap2() +
                 S1.integralSnap2() +
                 SM.integralSnap2();
        }

        real_type
        thetaMinMax( real_type & thMin, real_type & thMax ) const;

        real_type
        deltaTheta() const
        { real_type thMin, thMax; return thetaMinMax( thMin, thMax ); }

        real_type
        curvatureMinMax( 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 xBegin() const { return S0.xBegin(); }

        real_type yBegin() const { return S0.yBegin(); }

        real_type kappaBegin() const { return S0.kappaBegin(); }

        real_type thetaBegin() const { return S0.thetaBegin(); }

        real_type xEnd()const { return S1.xEnd(); }

        real_type yEnd() const { return S1.yEnd(); }

        real_type kappaEnd() const { return S1.kappaEnd(); }

        real_type thetaEnd() const { return S1.thetaEnd(); }

        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 ) {
          S0.rotate( angle, cx, cy );
          S1.rotate( angle, cx, cy );
          SM.rotate( angle, cx, cy );
        }

        void
        translate( real_type tx, real_type ty ){
          S0.translate( tx, ty );
          S1.translate( tx, ty );
          SM.translate( tx, ty );
        }

        void
        reverse() {
          ClothoidCurve tmp(S0); S1 = S0; S0 = tmp;
          S0.reverse();
          S1.reverse();
          SM.reverse();
        }

        friend
        ostream_type &
        operator << ( ostream_type & stream, ClothoidCurve const & c );

        void save( ostream_type & stream ) const;
      };

      /*\
       |   ____ _       _   _           _     _ _     _     _
       |  / ___| | ___ | |_| |__   ___ (_) __| | |   (_)___| |_
       | | |   | |/ _ \| __| '_ \ / _ \| |/ _` | |   | / __| __|
       | | |___| | (_) | |_| | | | (_) | | (_| | |___| \__ \ |_
       |  \____|_|\___/ \__|_| |_|\___/|_|\__,_|_____|_|___/\__|
       |
      \*/
      class ClothoidList : public BaseCurve {

        bool                  m_curve_is_closed;
        vector<real_type>     m_s0;
        vector<ClothoidCurve> m_clotoidList;

        mutable Utils::BinarySearch<int_type> m_lastInterval;

        mutable bool               m_aabb_done;
        mutable AABBtree           m_aabb_tree;
        mutable real_type          m_aabb_offs;
        mutable real_type          m_aabb_max_angle;
        mutable real_type          m_aabb_max_size;
        mutable vector<Triangle2D> m_aabb_tri;

        #ifndef DOXYGEN_SHOULD_SKIP_THIS
        class T2D_collision_list_ISO {
          ClothoidList const * pList1;
          real_type    const   m_offs1;
          ClothoidList const * pList2;
          real_type    const   m_offs2;
        public:
          T2D_collision_list_ISO(
            ClothoidList const * _pList1,
            real_type    const   _offs1,
            ClothoidList const * _pList2,
            real_type    const   _offs2
          )
          : pList1(_pList1)
          , m_offs1(_offs1)
          , pList2(_pList2)
          , m_offs2(_offs2)
          {}

          bool
          operator () ( BBox::PtrBBox ptr1, BBox::PtrBBox ptr2 ) const {
            Triangle2D    const & T1 = pList1->m_aabb_tri[size_t(ptr1->Ipos())];
            Triangle2D    const & T2 = pList2->m_aabb_tri[size_t(ptr2->Ipos())];
            ClothoidCurve const & C1 = pList1->get(T1.Icurve());
            ClothoidCurve const & C2 = pList2->get(T2.Icurve());
            real_type ss1, ss2;
            return C1.aabb_intersect_ISO( T1, m_offs1, &C2, T2, m_offs2, ss1, ss2 );
          }
        };
        #endif

        void
        resetLastInterval() {
          bool ok;
          int_type & lastInterval = *m_lastInterval.search( std::this_thread::get_id(), ok );
          lastInterval = 0;
        }

        int_type
        closestPoint_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()
        : BaseCurve(G2LIB_CLOTHOID_LIST)
        , m_curve_is_closed(false)
        , m_aabb_done(false)
        { this->resetLastInterval(); }

        ~ClothoidList() override {
          m_s0.clear();
          m_clotoidList.clear();
          m_aabb_tri.clear();
        }

        ClothoidList( ClothoidList const & s )
        : BaseCurve(G2LIB_CLOTHOID_LIST)
        , m_curve_is_closed(false)
        , m_aabb_done(false)
        { this->resetLastInterval(); copy(s); }

        void init();

        void reserve( int_type n );

        void copy( ClothoidList const & L );

        ClothoidList const & operator = ( ClothoidList const & s )
        { 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( BaseCurve const & C );

        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( 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->xEnd() - this->xBegin(); }

        real_type closure_gap_y()  const { return this->yEnd() - this->yBegin(); }

        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(
          int_type          n,
          real_type const * x,
          real_type const * y
        );

        bool
        build_G1(
          int_type          n,
          real_type const * x,
          real_type const * y,
          real_type const * theta
        );

        bool
        build(
          real_type         x0,
          real_type         y0,
          real_type         theta0,
          int_type          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,
            int_type(s.size()),
            &s.front(), &kappa.front()
          );
        }

        bool
        build_raw(
          int_type          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
        ) {
          int_type n = int_type(x.size());
          if ( n != int_type(y.size())        ||
               n != int_type(abscissa.size()) ||
               n != int_type(theta.size())    ||
               n != int_type(kappa.size()) ) return false;
          return build_raw(
            n, &x.front(), &y.front(),
            &abscissa.front(), &theta.front(), &kappa.front()
          );
        }

        ClothoidCurve const & get( int_type idx ) const;

        ClothoidCurve const & getAtS( real_type s ) const;

        int_type numSegments() const { return int_type(m_clotoidList.size()); }

        void wrap_in_range( real_type & s ) const;

        int_type findAtS( real_type & s ) const;

        // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

        real_type length() const override;
        real_type length_ISO( real_type offs ) const override;

        real_type
        segment_length( int_type nseg ) const;

        real_type
        segment_length_ISO( int_type nseg, real_type offs ) const;

        real_type
        segment_length_SAE( int_type nseg, real_type offs ) const
        { return segment_length_ISO( nseg, -offs ); }

        /*\
         |  _    _   _____    _                _
         | | |__| |_|_   _| _(_)__ _ _ _  __ _| |___
         | | '_ \ '_ \| || '_| / _` | ' \/ _` | / -_)
         | |_.__/_.__/|_||_| |_\__,_|_||_\__, |_\___|
         |                               |___/
        \*/

        void
        bbTriangles(
          std::vector<Triangle2D> & tvec,
          real_type                 max_angle = Utils::m_pi/6, // 30 degree
          real_type                 max_size  = 1e100,
          int_type                  icurve    = 0
        ) const override;

        void
        bbTriangles_ISO(
          real_type                 offs,
          std::vector<Triangle2D> & tvec,
          real_type                 max_angle = Utils::m_pi/6, // 30 degree
          real_type                 max_size  = 1e100,
          int_type                  icurve    = 0
        ) const override;

        void
        bbTriangles_SAE(
          real_type                 offs,
          std::vector<Triangle2D> & tvec,
          real_type                 max_angle = Utils::m_pi/6, // 30 degree
          real_type                 max_size  = 1e100,
          int_type                  icurve    = 0
        ) const override {
          this->bbTriangles_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
        thetaBegin() const override
        { return m_clotoidList.front().thetaBegin(); }

        real_type
        thetaEnd() const override
        { return m_clotoidList.back().thetaEnd(); }

        real_type
        xBegin() const override
        { return m_clotoidList.front().xBegin(); }

        real_type
        yBegin() const override
        { return m_clotoidList.front().yBegin(); }

        real_type
        xEnd() const override
        { return m_clotoidList.back().xEnd(); }

        real_type
        yEnd() const override
        { return m_clotoidList.back().yEnd(); }

        real_type
        xBegin_ISO( real_type offs ) const override
        { return m_clotoidList.front().xBegin_ISO( offs ); }

        real_type
        yBegin_ISO( real_type offs ) const override
        { return m_clotoidList.front().yBegin_ISO( offs ); }

        real_type xEnd_ISO( real_type offs ) const override
        { return m_clotoidList.back().xEnd_ISO( offs ); }

        real_type
        yEnd_ISO( real_type offs ) const override
        { return m_clotoidList.back().yEnd_ISO( offs ); }

        real_type tx_Begin() const override
        { return m_clotoidList.front().tx_Begin(); }

        real_type
        ty_Begin() const override
        { return m_clotoidList.front().ty_Begin(); }

        real_type
        tx_End() const override
        { return m_clotoidList.back().tx_End(); }

        real_type
        ty_End() const override
        { return m_clotoidList.back().ty_End(); }

        real_type
        nx_Begin_ISO() const override
        { return m_clotoidList.front().nx_Begin_ISO(); }

        real_type
        ny_Begin_ISO() const override
        { return m_clotoidList.front().ny_Begin_ISO(); }

        real_type
        nx_End_ISO() const override
        { return m_clotoidList.back().nx_End_ISO(); }

        real_type
        ny_End_ISO() const override
        { return m_clotoidList.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 changeOrigin( 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;

        /*\
         |      _ _     _
         |   __| (_)___| |_ __ _ _ __   ___ ___
         |  / _` | / __| __/ _` | '_ \ / __/ _ \
         | | (_| | \__ \ || (_| | | | | (_|  __/
         |  \__,_|_|___/\__\__,_|_| |_|\___\___|
        \*/

        int_type
        closestPoint_ISO(
          real_type   qx,
          real_type   qy,
          real_type & x,
          real_type & y,
          real_type & s,
          real_type & t,
          real_type & dst
        ) const override;

        int_type
        closestPoint_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;

        /*\
         |      _ _     _
         |   __| (_)___| |_ __ _ _ __   ___ ___
         |  / _` | / __| __/ _` | '_ \ / __/ _ \
         | | (_| | \__ \ || (_| | | | | (_|  __/
         |  \__,_|_|___/\__\__,_|_| |_|\___\___|
        \*/

        int_type
        closestSegment( real_type qx, real_type qy ) const;

        int_type
        closestPointInRange_ISO(
          real_type   qx,
          real_type   qy,
          int_type    icurve_begin,
          int_type    icurve_end,
          real_type & x,
          real_type & y,
          real_type & s,
          real_type & t,
          real_type & dst,
          int_type  & icurve
        ) const;

        int_type
        closestPointInRange_SAE(
          real_type   qx,
          real_type   qy,
          int_type    icurve_begin,
          int_type    icurve_end,
          real_type & x,
          real_type & y,
          real_type & s,
          real_type & t,
          real_type & dst,
          int_type  & icurve
        ) const {
          int_type res = this->closestPointInRange_ISO(
            qx, qy, icurve_begin, icurve_end, x, y, s, t, dst, icurve
          );
          t = -t;
          return res;
        }

        int_type
        closestPointInSRange_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,
          int_type  & icurve
        ) const;

        int_type
        closestPointInSRange_SAE(
          real_type   qx,
          real_type   qy,
          int_type    s_begin,
          int_type    s_end,
          real_type & x,
          real_type & y,
          real_type & s,
          real_type & t,
          real_type & dst,
          int_type  & icurve
        ) const {
          int_type res = this->closestPointInSRange_ISO(
            qx, qy, s_begin, s_end, x, y, s, t, dst, icurve
          );
          t = -t;
          return res;
        }

        void
        info( ostream_type & stream ) const override
        { stream << "ClothoidList\n" << *this << '\n'; }

        friend
        ostream_type &
        operator << ( ostream_type & stream, ClothoidList const & CL );

        void
        getSK( real_type * s, real_type * kappa ) const;

        void
        getSK(
          std::vector<real_type> & s,
          std::vector<real_type> & kappa
        ) const {
          s.resize( m_clotoidList.size()+1 );
          kappa.resize( m_clotoidList.size()+1 );
          getSK( &s.front(), &kappa.front() );
        }

        void
        getSTK(
          real_type * s,
          real_type * theta,
          real_type * kappa
        ) const;

        void
        getSTK(
          std::vector<real_type> & s,
          std::vector<real_type> & theta,
          std::vector<real_type> & kappa
        ) const {
          s.resize( m_clotoidList.size()+1 );
          theta.resize( m_clotoidList.size()+1 );
          kappa.resize( m_clotoidList.size()+1 );
          getSTK( &s.front(), &theta.front(), &kappa.front() );
        }

        void
        getXY( real_type * x, real_type * y ) const;

        void
        getDeltaTheta( real_type * deltaTheta ) const;

        void
        getDeltaKappa( real_type * deltaKappa ) const;

        int_type
        findST1(
          real_type   x,
          real_type   y,
          real_type & s,
          real_type & t
        ) const;

        int_type
        findST1(
          int_type    ibegin,
          int_type    iend,
          real_type   x,
          real_type   y,
          real_type & s,
          real_type & t
        ) const;

        /*\
         |             _ _ _     _
         |    ___ ___ | | (_)___(_) ___  _ __
         |   / __/ _ \| | | / __| |/ _ \| '_ \
         |  | (_| (_) | | | \__ \ | (_) | | | |
         |   \___\___/|_|_|_|___/_|\___/|_| |_|
        \*/

        bool
        collision( ClothoidList const & C ) const;

        bool
        collision_ISO(
          real_type            offs,
          ClothoidList const & CL,
          real_type            offs_C
        ) const;

        /*\
         |   _       _                          _
         |  (_)_ __ | |_ ___ _ __ ___  ___  ___| |_
         |  | | '_ \| __/ _ \ '__/ __|/ _ \/ __| __|
         |  | | | | | ||  __/ |  \__ \  __/ (__| |_
         |  |_|_| |_|\__\___|_|  |___/\___|\___|\__|
        \*/

        void
        intersect(
          ClothoidList const & CL,
          IntersectList      & ilist,
          bool                 swap_s_vals
        ) const {
          intersect_ISO( 0, CL, 0, ilist, swap_s_vals );
        }

        void
        intersect_ISO(
          real_type            offs,
          ClothoidList const & CL,
          real_type            offs_obj,
          IntersectList      & ilist,
          bool                 swap_s_vals
        ) const;

        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 );

      };

      /*\
       |
       |    ___ _     _   _        _    _ ___      _ _           ___ ___
       |   / __| |___| |_| |_  ___(_)__| / __|_ __| (_)_ _  ___ / __|_  )
       |  | (__| / _ \  _| ' \/ _ \ / _` \__ \ '_ \ | | ' \/ -_) (_ |/ /
       |   \___|_\___/\__|_||_\___/_\__,_|___/ .__/_|_|_||_\___|\___/___|
       |                                     |_|
      \*/

      class ClothoidSplineG2 {
      public:
        typedef enum { P1 = 1, P2, P3, P4, P5, P6, P7, P8, P9 } TargetType;

      private:

        Utils::Malloc<real_type> realValues;

        real_type * m_x;
        real_type * m_y;
        TargetType  m_tt;
        real_type   m_theta_I;
        real_type   m_theta_F;
        int_type    m_npts;

        // work vector
        mutable real_type * m_k;
        mutable real_type * m_dk;
        mutable real_type * m_L;
        mutable real_type * m_kL;
        mutable real_type * m_L_1;
        mutable real_type * m_L_2;
        mutable real_type * m_k_1;
        mutable real_type * m_k_2;
        mutable real_type * m_dk_1;
        mutable real_type * m_dk_2;

        real_type
        diff2pi( real_type in ) const {
          return in-Utils::m_2pi*round(in/Utils::m_2pi);
        }

      public:

        ClothoidSplineG2()
        : realValues("ClothoidSplineG2"), m_tt(P1)
        {}

        ~ClothoidSplineG2() {}

        void
        setP1( real_type theta0, real_type thetaN )
        { m_tt = P1; m_theta_I = theta0; m_theta_F = thetaN; }

        void setP2() { m_tt = P2; }
        void setP3() { m_tt = P3; }
        void setP4() { m_tt = P4; }
        void setP5() { m_tt = P5; }
        void setP6() { m_tt = P6; }
        void setP7() { m_tt = P7; }
        void setP8() { m_tt = P8; }
        void setP9() { m_tt = P9; }

        void
        build(
          real_type const * xvec,
          real_type const * yvec,
          int_type          npts
        );

        int_type numPnts() const { return m_npts; }
        int_type numTheta() const;
        int_type 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;

        int_type
        jacobian_nnz() const;

        bool
        jacobian_pattern( int_type * i, int_type * j ) const;

        bool
        jacobian_pattern_matlab( real_type * i, real_type * j ) const;

        bool
        jacobian( real_type const * theta, real_type * vals ) const;

        void
        info( ostream_type & stream ) const
        { stream << "ClothoidSplineG2\n" << *this << '\n'; }

        friend
        ostream_type &
        operator << ( ostream_type & stream, ClothoidSplineG2 const & c );

      };

    }