# Program Listing for File ClothoidList.cc

↰ Return to documentation for file (`ClothoidList.cc`)

    /*--------------------------------------------------------------------------*\
     |                                                                          |
     |  Copyright (C) 2017                                                      |
     |                                                                          |
     |         , __                 , __                                        |
     |        /|/  \               /|/  \                                       |
     |         | __/ _   ,_         | __/ _   ,_                                |
     |         |   \|/  /  |  |   | |   \|/  /  |  |   |                        |
     |         |(__/|__/   |_/ \_/|/|(__/|__/   |_/ \_/|/                       |
     |                           /|                   /|                        |
     |                           \|                   \|                        |
     |                                                                          |
     |      Enrico Bertolazzi                                                   |
     |      Dipartimento di Ingegneria Industriale                              |
     |      Universita` degli Studi di Trento                                   |
     |      email: enrico.bertolazzi@unitn.it                                   |
     |                                                                          |
    \*--------------------------------------------------------------------------*/

    #include "Clothoids.hh"

    #include <cfloat>
    #include <limits>

    #ifdef __GNUC__
    #pragma GCC diagnostic push
    #pragma GCC diagnostic ignored "-Wsign-conversion"
    #endif
    #ifdef __clang__
    #pragma clang diagnostic push
    #pragma clang diagnostic ignored "-Wsign-conversion"
    #endif

    namespace G2lib {

      using std::numeric_limits;
      using std::lower_bound;
      using std::vector;
      using std::swap;
      using std::abs;

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

      ClothoidList::ClothoidList( LineSegment const & LS )
      : BaseCurve(G2LIB_CLOTHOID_LIST)
      , m_curve_is_closed(false)
      , m_aabb_done(false)
      {
        this->resetLastInterval();
        this->init();
        this->push_back( LS );
      }

      ClothoidList::ClothoidList( CircleArc const & C )
      : BaseCurve(G2LIB_CLOTHOID_LIST)
      , m_curve_is_closed(false)
      , m_aabb_done(false)
      {
        this->resetLastInterval();
        this->init();
        this->push_back( C );
      }

      ClothoidList::ClothoidList( Biarc const & C )
      : BaseCurve(G2LIB_CLOTHOID_LIST)
      , m_curve_is_closed(false)
      , m_aabb_done(false)
      {
        this->resetLastInterval();
        this->init();
        this->push_back( C.C0() );
        this->push_back( C.C1() );
      }

      ClothoidList::ClothoidList( BiarcList const & c )
      : BaseCurve(G2LIB_CLOTHOID_LIST)
      , m_curve_is_closed(false)
      , m_aabb_done(false)
      {
        this->resetLastInterval();
        this->init();
        this->push_back( c );
      }

      ClothoidList::ClothoidList( ClothoidCurve const & c )
      : BaseCurve(G2LIB_CLOTHOID_LIST)
      , m_curve_is_closed(false)
      , m_aabb_done(false)
      {
        this->resetLastInterval();
        this->init();
        this->push_back( c );
      }

      ClothoidList::ClothoidList( PolyLine const & pl )
      : BaseCurve(G2LIB_CLOTHOID_LIST)
      , m_curve_is_closed(false)
      , m_aabb_done(false)
      {
        this->resetLastInterval();
        this->init();
        this->push_back( pl );
      }

      ClothoidList::ClothoidList( BaseCurve const & C )
      : BaseCurve(G2LIB_CLOTHOID_LIST)
      , m_curve_is_closed(false)
      , m_aabb_done(false)
      {
        this->resetLastInterval();
        this->init();
        switch ( C.type() ) {
        case G2LIB_LINE:
          push_back( *static_cast<LineSegment const *>(&C) );
          break;
        case G2LIB_CIRCLE:
          push_back( *static_cast<CircleArc const *>(&C) );
          break;
        case G2LIB_CLOTHOID:
          push_back( *static_cast<ClothoidCurve const *>(&C) );
          break;
        case G2LIB_BIARC:
          push_back( *static_cast<Biarc const *>(&C) );
          break;
        case G2LIB_BIARC_LIST:
          push_back( *static_cast<BiarcList const *>(&C) );
          break;
        case G2LIB_CLOTHOID_LIST:
          copy( *static_cast<ClothoidList const *>(&C) );
          break;
        case G2LIB_POLYLINE:
          push_back( *static_cast<PolyLine const *>(&C) );
          break;
        }
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      int_type
      ClothoidList::findAtS( real_type & s ) const {
        bool ok;
        int_type & lastInterval = *m_lastInterval.search( std::this_thread::get_id(), ok );
        Utils::searchInterval<int_type,real_type>(
          static_cast<int_type>(m_s0.size()),
          &m_s0.front(), s, lastInterval, m_curve_is_closed, true
        );
        return lastInterval;
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::wrap_in_range( real_type & s ) const {
        real_type a = m_s0.front();
        real_type b = m_s0.back();
        real_type L = b-a;
        s -= a;
        s  = fmod( s, L );
        if ( s < 0 ) s += L;
        // while ( s < 0 ) s += L;
        // while ( s > L ) s -= L;
        s += a;
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::init() {
        m_s0.clear();
        m_clotoidList.clear();
        this->resetLastInterval();
        m_aabb_done = false;
        this->resetLastInterval();
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::copy( ClothoidList const & L ) {
        this->init();
        m_clotoidList.reserve( L.m_clotoidList.size() );
        std::copy(
          L.m_clotoidList.begin(), L.m_clotoidList.end(), back_inserter(m_clotoidList)
        );
        m_s0.reserve( L.m_s0.size() );
        std::copy( L.m_s0.begin(), L.m_s0.end(), back_inserter(m_s0) );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::reserve( int_type n ) {
        m_s0.reserve(size_t(n+1));
        m_clotoidList.reserve(size_t(n));
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::push_back( LineSegment const & LS ) {
        if ( m_clotoidList.empty() ) {
          m_s0.push_back(0);
          m_s0.push_back(LS.length());
        } else {
          m_s0.push_back(m_s0.back()+LS.length());
        }
        m_clotoidList.push_back(ClothoidCurve(LS));
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::push_back( CircleArc const & C ) {
        if ( m_clotoidList.empty() ) {
          m_s0.push_back(0);
          m_s0.push_back(C.length());
        } else {
          m_s0.push_back(m_s0.back()+C.length());
        }
        m_clotoidList.push_back(ClothoidCurve(C));
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::push_back( Biarc const & c ) {
        if ( m_clotoidList.empty() ) m_s0.push_back( 0 );
        CircleArc const & C0 = c.C0();
        CircleArc const & C1 = c.C1();
        m_s0.push_back( m_s0.back()+C0.length() );
        m_s0.push_back( m_s0.back()+C1.length() );
        m_clotoidList.push_back( ClothoidCurve(C0) );
        m_clotoidList.push_back( ClothoidCurve(C1) );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::push_back( ClothoidCurve const & c ) {
        if ( m_clotoidList.empty() ) {
          m_s0.push_back(0);
          m_s0.push_back(c.length());
        } else {
          m_s0.push_back(m_s0.back()+c.length());
        }
        m_clotoidList.push_back(c);
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::push_back( BiarcList const & c ) {
        m_s0.reserve( m_s0.size() + c.m_biarcList.size() + 1 );
        m_clotoidList.reserve( m_clotoidList.size() + 2*c.m_biarcList.size() );

        if ( m_s0.empty() ) m_s0.push_back(0);

        vector<Biarc>::const_iterator ip = c.m_biarcList.begin();
        for (; ip != c.m_biarcList.end(); ++ip ) {
          m_s0.push_back(m_s0.back()+ip->length());
          Biarc const & b = *ip;
          m_clotoidList.push_back(ClothoidCurve(b.C0()));
          m_clotoidList.push_back(ClothoidCurve(b.C1()));
        }
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::push_back( PolyLine const & c ) {
        m_s0.reserve( m_s0.size() + c.m_polylineList.size() + 1 );
        m_clotoidList.reserve( m_clotoidList.size() + c.m_polylineList.size() );

        if ( m_s0.empty() ) m_s0.push_back(0);

        vector<LineSegment>::const_iterator ip = c.m_polylineList.begin();
        for (; ip != c.m_polylineList.end(); ++ip ) {
          m_s0.push_back(m_s0.back()+ip->length());
          m_clotoidList.push_back(ClothoidCurve(*ip));
        }
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::push_back( ClothoidList const & c ) {
        m_s0.reserve( m_s0.size() + c.m_clotoidList.size() + 1 );
        m_clotoidList.reserve( m_clotoidList.size() + c.m_clotoidList.size() );

        if ( m_s0.empty() ) m_s0.push_back(0);

        vector<ClothoidCurve>::const_iterator ip = c.m_clotoidList.begin();
        for (; ip != c.m_clotoidList.end(); ++ip ) {
          m_s0.push_back(m_s0.back()+ip->length());
          m_clotoidList.push_back(*ip);
        }
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::push_back(
        real_type kappa0,
        real_type dkappa,
        real_type L
      ) {
        UTILS_ASSERT0(
          !m_clotoidList.empty(),
          "ClothoidList::push_back_G1(...) empty list!\n"
        );
        ClothoidCurve c;
        real_type x0     = m_clotoidList.back().xEnd();
        real_type y0     = m_clotoidList.back().yEnd();
        real_type theta0 = m_clotoidList.back().thetaEnd();
        c.build( x0, y0, theta0, kappa0, dkappa, L );
        push_back( c );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::push_back(
        real_type x0,
        real_type y0,
        real_type theta0,
        real_type kappa0,
        real_type dkappa,
        real_type L
      ) {
        ClothoidCurve c;
        c.build( x0, y0, theta0, kappa0, dkappa, L );
        push_back( c );
      }
      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::push_back_G1(
        real_type x1,
        real_type y1,
        real_type theta1
      ) {
        UTILS_ASSERT0(
          !m_clotoidList.empty(),
          "ClothoidList::push_back_G1(...) empty list!\n"
        );
        ClothoidCurve c;
        real_type x0     = m_clotoidList.back().xEnd();
        real_type y0     = m_clotoidList.back().yEnd();
        real_type theta0 = m_clotoidList.back().thetaEnd();
        c.build_G1( x0, y0, theta0, x1, y1, theta1 );
        push_back( c );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::push_back_G1(
        real_type x0,
        real_type y0,
        real_type theta0,
        real_type x1,
        real_type y1,
        real_type theta1
      ) {
        ClothoidCurve c;
        c.build_G1( x0, y0, theta0, x1, y1, theta1 );
        push_back( c );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      bool
      ClothoidList::build_G1(
        int_type          n,
        real_type const * x,
        real_type const * y
      ) {
        init();
        reserve( n-1 );
        ClothoidCurve c;

        UTILS_ASSERT0(
          n > 1, "ClothoidList::build_G1, at least 2 points are necessary\n"
        );

        if ( n == 2 ) {

          real_type theta = atan2( y[1] - y[0], x[1] - x[0] );
          c.build_G1( x[0], y[0], theta, x[1], y[1], theta );
          push_back(c);

        } else {

          Biarc b;
          bool ok, ciclic = hypot( x[0]-x[n-1], y[0]-y[n-1] ) < 1e-10;
          real_type thetaC(0);
          if ( ciclic ) {
            ok = b.build_3P( x[n-2], y[n-2], x[0], y[0], x[1], y[1] );
            UTILS_ASSERT0( ok, "ClothoidList::build_G1, failed\n" );
            thetaC = b.thetaMiddle();
          }
          ok = b.build_3P( x[0], y[0], x[1], y[1], x[2], y[2] );
          UTILS_ASSERT0( ok, "ClothoidList::build_G1, failed\n" );
          real_type theta0 = ciclic ? thetaC : b.thetaBegin();
          real_type theta1 = b.thetaMiddle();
          c.build_G1( x[0], y[0], theta0, x[1], y[1], theta1 );
          push_back(c);
          for ( int_type k = 2; k < n-1; ++k ) {
            theta0 = theta1;
            ok = b.build_3P( x[k-1], y[k-1], x[k], y[k], x[k+1], y[k+1] );
            UTILS_ASSERT0( ok, "ClothoidList::build_G1, failed\n" );
            theta1 = b.thetaMiddle();
            c.build_G1( x[k-1], y[k-1], theta0, x[k], y[k], theta1 );
            push_back(c);
          }
          theta0 = theta1;
          theta1 = ciclic ? thetaC : b.thetaEnd();
          c.build_G1( x[n-2], y[n-2], theta0, x[n-1], y[n-1], theta1 );
          push_back(c);

        }
        return true;
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      bool
      ClothoidList::build_G1(
        int_type          n,
        real_type const * x,
        real_type const * y,
        real_type const * theta
      ) {

        UTILS_ASSERT0(
          n > 1, "ClothoidList::build_G1, at least 2 points are necessary\n"
        );

        init();
        reserve( n-1 );
        ClothoidCurve c;
        for ( int_type k = 1; k < n; ++k ) {
          c.build_G1( x[k-1], y[k-1], theta[k-1], x[k], y[k], theta[k] );
          push_back(c);
        }
        return true;
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      bool
      ClothoidList::build(
        real_type         x0,
        real_type         y0,
        real_type         theta0,
        int_type          n,
        real_type const * s,
        real_type const * kappa
      ) {
        if ( n < 2 ) return false;
        real_type tol = abs(s[n-1]-s[0])*machepsi10; // minimum admissible length

        init();
        real_type k  = kappa[0];
        real_type L  = s[1]-s[0];
        real_type dk = (kappa[1]-k)/L;
        UTILS_ASSERT(
          Utils::isRegular( k ) && Utils::isRegular( L ) && Utils::isRegular( dk ),
          "ClothoidList::build, failed first segment found\n"
          "L = {} k = {} dk = {}\n",
          L, k, dk
        );
        push_back( x0, y0, theta0, k, dk, L );
        for ( int_type i = 2; i < n; ++i ) {
          k  = kappa[i-1];
          L  = s[i]-s[i-1];
          if ( abs(L) < tol ) {
            fmt::print( "ClothoidList::build, skipping segment N.{}\n", i);
            continue; // skip too small segment
          }
          dk = (kappa[i]-k)/L;
          UTILS_ASSERT(
            Utils::isRegular( k ) && Utils::isRegular( L ) && Utils::isRegular( dk ),
            "ClothoidList::build, failed at segment N.{} found\n"
            "L = {} k = {} dk = {}\n",
            i, L, k, dk
          );
          push_back( k, dk, L );
        }
        return true;
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      bool
      ClothoidList::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
      ) {
        if ( n < 2 ) return false;
        init();
        m_clotoidList.reserve(size_t(n-1));
        real_type const * px = x;
        real_type const * py = y;
        real_type const * pa = abscissa;
        real_type const * pt = theta;
        real_type const * pk = kappa;
        for ( int_type i = 1; i < n; ++i, ++px, ++py, ++pa, ++pt, ++pk ) {
          real_type dk  = pk[1]-pk[0];
          real_type L   = pa[1]-pa[0];
          push_back( *px, *py, *pt, *pk, dk, L );
        }
        return true;
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      ClothoidCurve const &
      ClothoidList::get( int_type idx ) const {
        UTILS_ASSERT(
          !m_clotoidList.empty(), "ClothoidList::get( {} ) empty list\n", idx
        );
        UTILS_ASSERT(
          idx >= 0 && idx < int_type(m_clotoidList.size()),
          "ClothoidList::get( {} ) bad index, must be in [0,{}]\n",
          idx, m_clotoidList.size()-1
        );
        return m_clotoidList[idx];
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      ClothoidCurve const &
      ClothoidList::getAtS( real_type s ) const {
        int_type idx = this->findAtS(s);
        return get( idx );
      }

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

      real_type
      ClothoidList::length() const
      {
        if ( m_clotoidList.empty() ) return 0.0;
        return m_s0.back() - m_s0.front();
      }

      real_type
      ClothoidList::length_ISO( real_type offs ) const {
        real_type L = 0;
        vector<ClothoidCurve>::const_iterator is = m_clotoidList.begin();
        for (; is != m_clotoidList.end(); ++is ) L += is->length_ISO( offs );
        return L;
      }

      real_type
      ClothoidList::segment_length( int_type nseg ) const {
        ClothoidCurve const & c = get( nseg );
        return c.length();
      }

      real_type
      ClothoidList::segment_length_ISO( int_type nseg, real_type offs ) const {
        ClothoidCurve const & c = get( nseg );
        return c.length_ISO( offs );
      }

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

      void
      ClothoidList::bbTriangles(
        vector<Triangle2D> & tvec,
        real_type            max_angle,
        real_type            max_size,
        int_type             icurve
      ) const {
        vector<ClothoidCurve>::const_iterator ic = m_clotoidList.begin();
        for ( int_type ipos = icurve; ic != m_clotoidList.end(); ++ic, ++ipos )
          ic->bbTriangles( tvec, max_angle, max_size, ipos );
      }

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

      void
      ClothoidList::bbTriangles_ISO(
        real_type            offs,
        vector<Triangle2D> & tvec,
        real_type            max_angle,
        real_type            max_size,
        int_type             icurve
      ) const {
        vector<ClothoidCurve>::const_iterator ic = m_clotoidList.begin();
        for ( int_type ipos = icurve; ic != m_clotoidList.end(); ++ic, ++ipos )
          ic->bbTriangles_ISO( offs, tvec, max_angle, max_size, ipos );
      }

      /*\
       |   _     _
       |  | |__ | |__   _____  __
       |  | '_ \| '_ \ / _ \ \/ /
       |  | |_) | |_) | (_) >  <
       |  |_.__/|_.__/ \___/_/\_\
      \*/

      void
      ClothoidList::bbox_ISO(
        real_type   offs,
        real_type & xmin,
        real_type & ymin,
        real_type & xmax,
        real_type & ymax
      ) const {
        vector<Triangle2D> tvec;
        bbTriangles_ISO( offs, tvec, Utils::m_pi/18, 1e100 );
        xmin = ymin = numeric_limits<real_type>::infinity();
        xmax = ymax = -xmin;
        vector<Triangle2D>::const_iterator it;
        for ( it = tvec.begin(); it != tvec.end(); ++it ) {
          // - - - - - - - - - - - - - - - - - - - -
          if      ( it->x1() < xmin ) xmin = it->x1();
          else if ( it->x1() > xmax ) xmax = it->x1();
          if      ( it->x2() < xmin ) xmin = it->x2();
          else if ( it->x2() > xmax ) xmax = it->x2();
          if      ( it->x3() < xmin ) xmin = it->x3();
          else if ( it->x3() > xmax ) xmax = it->x3();
          // - - - - - - - - - - - - - - - - - - - -
          if      ( it->y1() < ymin ) ymin = it->y1();
          else if ( it->y1() > ymax ) ymax = it->y1();
          if      ( it->y2() < ymin ) ymin = it->y2();
          else if ( it->y2() > ymax ) ymax = it->y2();
          if      ( it->y3() < ymin ) ymin = it->y3();
          else if ( it->y3() > ymax ) ymax = it->y3();
        }
      }

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

      real_type
      ClothoidList::theta( real_type s ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.theta( s - m_s0[idx] );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      real_type
      ClothoidList::theta_D( real_type s ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.theta_D( s - m_s0[idx] );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      real_type
      ClothoidList::theta_DD( real_type s ) const  {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.theta_DD( s - m_s0[idx] );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      real_type
      ClothoidList::theta_DDD( real_type s ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.theta_DDD( s - m_s0[idx] );
      }

      /*\
       |  _____                   _    _   _
       | |_   _|   __ _ _ __   __| |  | \ | |
       |   | |    / _` | '_ \ / _` |  |  \| |
       |   | |   | (_| | | | | (_| |  | |\  |
       |   |_|    \__,_|_| |_|\__,_|  |_| \_|
      \*/

      real_type
      ClothoidList::tx( real_type s ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.tx( s - m_s0[idx] );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      real_type
      ClothoidList::ty( real_type s ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.ty( s - m_s0[idx] );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      real_type
      ClothoidList::tx_D( real_type s ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.tx_D( s - m_s0[idx] );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      real_type
      ClothoidList::ty_D( real_type s ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.ty_D( s - m_s0[idx] );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      real_type
      ClothoidList::tx_DD( real_type s ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.tx_DD( s - m_s0[idx] );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      real_type
      ClothoidList::ty_DD( real_type s ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.ty_DD( s - m_s0[idx] );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      real_type
      ClothoidList::tx_DDD( real_type s ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.tx_DDD( s - m_s0[idx] );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      real_type
      ClothoidList::ty_DDD( real_type s ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.ty_DDD( s - m_s0[idx] );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::tg(
        real_type   s,
        real_type & tg_x,
        real_type & tg_y
      ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.tg( s - m_s0[idx], tg_x, tg_y );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::tg_D(
        real_type   s,
        real_type & tg_x_D,
        real_type & tg_y_D
      ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.tg_D( s - m_s0[idx], tg_x_D, tg_y_D );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::tg_DD(
        real_type   s,
        real_type & tg_x_DD,
        real_type & tg_y_DD
      ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.tg_DD( s - m_s0[idx], tg_x_DD, tg_y_DD );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::tg_DDD(
        real_type   s,
        real_type & tg_x_DDD,
        real_type & tg_y_DDD
      ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.tg_DDD( s - m_s0[idx], tg_x_DDD, tg_y_DDD );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::evaluate(
        real_type   s,
        real_type & th,
        real_type & k,
        real_type & x,
        real_type & y
      ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        c.evaluate( s - m_s0[idx], th, k, x, y );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::evaluate_ISO(
        real_type   s,
        real_type   offs,
        real_type & th,
        real_type & k,
        real_type & x,
        real_type & y
      ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        c.evaluate_ISO( s - m_s0[idx], offs, th, k, x, y );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      real_type
      ClothoidList::X( real_type s ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.X( s - m_s0[idx] );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      real_type
      ClothoidList::Y( real_type s ) const  {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.Y( s - m_s0[idx] );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      real_type
      ClothoidList::X_D( real_type s ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.X_D( s - m_s0[idx] );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      real_type
      ClothoidList::Y_D( real_type s ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.Y_D( s - m_s0[idx] );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      real_type
      ClothoidList::X_DD( real_type s ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.X_DD( s - m_s0[idx] );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      real_type
      ClothoidList::Y_DD( real_type s ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.Y_DD( s - m_s0[idx] );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      real_type
      ClothoidList::X_DDD( real_type s ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.X_DDD( s - m_s0[idx] );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      real_type
      ClothoidList::Y_DDD( real_type s ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.Y_DDD( s - m_s0[idx] );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::eval(
        real_type   s,
        real_type & x,
        real_type & y
      ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.eval( s - m_s0[idx], x, y );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::eval_D(
        real_type   s,
        real_type & x_D,
        real_type & y_D
      ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.eval_D( s - m_s0[idx], x_D, y_D );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::eval_DD(
        real_type   s,
        real_type & x_DD,
        real_type & y_DD
      ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.eval_DD( s - m_s0[idx], x_DD, y_DD );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::eval_DDD(
        real_type   s,
        real_type & x_DDD,
        real_type & y_DDD
      ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.eval_DDD( s - m_s0[idx], x_DDD, y_DDD );
      }

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

      real_type
      ClothoidList::X_ISO( real_type s, real_type offs ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.X_ISO( s - m_s0[idx], offs );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      real_type
      ClothoidList::Y_ISO( real_type s, real_type offs ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.Y_ISO( s - m_s0[idx], offs );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      real_type
      ClothoidList::X_ISO_D( real_type s, real_type offs ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.X_ISO_D( s - m_s0[idx], offs );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      real_type
      ClothoidList::Y_ISO_D( real_type s, real_type offs ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.Y_ISO_D( s - m_s0[idx], offs );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      real_type
      ClothoidList::X_ISO_DD( real_type s, real_type offs ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.X_ISO_DD( s - m_s0[idx], offs );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      real_type
      ClothoidList::Y_ISO_DD( real_type s, real_type offs ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.Y_ISO_DD( s - m_s0[idx], offs );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      real_type
      ClothoidList::X_ISO_DDD( real_type s, real_type offs ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.X_ISO_DDD( s - m_s0[idx], offs );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      real_type
      ClothoidList::Y_ISO_DDD( real_type s, real_type offs ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.Y_ISO_DDD( s - m_s0[idx], offs );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::eval_ISO(
        real_type   s,
        real_type   offs,
        real_type & x,
        real_type & y
      ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.eval_ISO( s - m_s0[idx], offs, x, y );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::eval_ISO_D(
        real_type   s,
        real_type   offs,
        real_type & x_D,
        real_type & y_D
      ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.eval_ISO_D( s - m_s0[idx], offs, x_D, y_D );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::eval_ISO_DD(
        real_type   s,
        real_type   offs,
        real_type & x_DD,
        real_type & y_DD
      ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.eval_ISO_DD( s - m_s0[idx], offs, x_DD, y_DD );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::eval_ISO_DDD(
        real_type   s,
        real_type   offs,
        real_type & x_DDD,
        real_type & y_DDD
      ) const {
        int_type idx = findAtS( s );
        ClothoidCurve const & c = get( idx );
        return c.eval_ISO_DDD( s - m_s0[idx], offs, x_DDD, y_DDD );
      }

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

      void
      ClothoidList::translate( real_type tx, real_type ty ) {
        vector<ClothoidCurve>::iterator ic = m_clotoidList.begin();
        for (; ic != m_clotoidList.end(); ++ic ) ic->translate( tx, ty );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::rotate( real_type angle, real_type cx, real_type cy ) {
        vector<ClothoidCurve>::iterator ic = m_clotoidList.begin();
        for (; ic != m_clotoidList.end(); ++ic ) ic->rotate( angle, cx, cy );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::scale( real_type sfactor ) {
        vector<ClothoidCurve>::iterator ic = m_clotoidList.begin();
        real_type newx0 = ic->xBegin();
        real_type newy0 = ic->yBegin();
        m_s0[0] = 0;
        for ( size_t k=0; ic != m_clotoidList.end(); ++ic, ++k ) {
          ic->scale( sfactor );
          ic->changeOrigin( newx0, newy0 );
          newx0     = ic->xEnd();
          newy0     = ic->yEnd();
          m_s0[k+1] = m_s0[k] + ic->length();
        }
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::reverse() {
        std::reverse( m_clotoidList.begin(), m_clotoidList.end() );
        vector<ClothoidCurve>::iterator ic = m_clotoidList.begin();
        ic->reverse();
        real_type newx0 = ic->xEnd();
        real_type newy0 = ic->yEnd();
        m_s0[0] = 0;
        m_s0[1] = ic->length();
        size_t k = 1;
        for ( ++ic; ic != m_clotoidList.end(); ++ic, ++k ) {
          ic->reverse();
          ic->changeOrigin( newx0, newy0 );
          newx0     = ic->xEnd();
          newy0     = ic->yEnd();
          m_s0[k+1] = m_s0[k] + ic->length();
        }
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::changeOrigin( real_type newx0, real_type newy0 ) {
        vector<ClothoidCurve>::iterator ic = m_clotoidList.begin();
        for (; ic != m_clotoidList.end(); ++ic ) {
          ic->changeOrigin( newx0, newy0 );
          newx0 = ic->xEnd();
          newy0 = ic->yEnd();
        }
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::trim( real_type s_begin, real_type s_end ) {

        ClothoidList newCL;
        this->trim( s_begin, s_end, newCL );
        this->copy( newCL );

    #if 0
        UTILS_ASSERT(
          s_begin >= m_s0.front() && s_end <= m_s0.back() && s_end > s_begin,
          "ClothoidList::trim( s_begin={}, s_end={} ) bad range, must be in [{},{}]\n",
          s_begin, s_end, m_s0.front(), m_s0.back()
        );

        size_t i_begin = size_t( findAtS( s_begin ) );
        size_t i_end   = size_t( findAtS( s_end   ) );
        if ( i_begin == i_end ) {
          m_clotoidList[i_begin].trim( s_begin-m_s0[i_begin], s_end-m_s0[i_begin] );
        } else {
          m_clotoidList[i_begin].trim( s_begin-m_s0[i_begin], m_s0[i_begin+1]-m_s0[i_begin] );
          m_clotoidList[i_end].trim( 0, s_end-m_s0[i_end] );
        }
        m_clotoidList.erase( m_clotoidList.begin()+i_end+1, m_clotoidList.end() );
        m_clotoidList.erase( m_clotoidList.begin(), m_clotoidList.begin()+i_begin );
        if ( m_clotoidList.back().m_L <= machepsi100 ) m_clotoidList.pop_back();
        vector<ClothoidCurve>::iterator ic = m_clotoidList.begin();
        m_s0.resize( m_clotoidList.size() + 1 );
        m_s0[0] = 0;
        size_t k = 0;
        for (; ic != m_clotoidList.end(); ++ic, ++k )
          m_s0[k+1] = m_s0[k] + ic->length();
        this->resetLastInterval();
    #endif
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::trim( real_type s_begin, real_type s_end, ClothoidList & newCL ) const {

        newCL.init();

        if ( m_clotoidList.empty() ) return;

        // put in range
        real_type L = this->length();
        while ( s_begin > L ) s_begin -= L;
        while ( s_begin < 0 ) s_begin += L;
        while ( s_end   > L ) s_end   -= L;
        while ( s_end   < 0 ) s_end   += L;

        int_type n_seg   = int_type( m_clotoidList.size() );
        int_type i_begin = findAtS( s_begin );
        int_type i_end   = findAtS( s_end );

        if ( s_begin < s_end ) {
          // get initial and final segment
          if ( i_begin == i_end ) { // stesso segmento
            real_type   ss0 = m_s0[i_begin];
            ClothoidCurve C = m_clotoidList[i_begin];
            C.trim( s_begin-ss0, s_end-ss0 );
            newCL.push_back( C );
          } else {
            ClothoidCurve C0 = m_clotoidList[i_begin];
            C0.trim( s_begin - m_s0[i_begin], C0.length() );
            newCL.push_back( C0 );

            for ( ++i_begin; i_begin < i_end; ++i_begin )
              newCL.push_back( m_clotoidList[i_begin] );

            ClothoidCurve C1 = m_clotoidList[i_end];
            C1.trim( 0, s_end - m_s0[i_end] );
            newCL.push_back( C1 );
          }
        } else {
          ClothoidCurve C0 = m_clotoidList[i_begin];
          C0.trim( s_begin - m_s0[i_begin], C0.length() );
          newCL.push_back( C0 );

          for ( ++i_begin; i_begin < n_seg; ++i_begin )
            newCL.push_back( m_clotoidList[i_begin] );

          for ( i_begin = 0; i_begin < i_end; ++i_begin )
            newCL.push_back( m_clotoidList[i_begin] );

          ClothoidCurve C1 = m_clotoidList[i_end];
          C1.trim( 0, s_end - m_s0[i_end] );
          newCL.push_back( C1 );
        }
      }

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

      #ifndef DOXYGEN_SHOULD_SKIP_THIS
      void
      ClothoidList::build_AABBtree_ISO(
        real_type offs,
        real_type max_angle,
        real_type max_size
      ) const {

        if ( m_aabb_done &&
             Utils::isZero( offs-m_aabb_offs ) &&
             Utils::isZero( max_angle-m_aabb_max_angle ) &&
             Utils::isZero( max_size-m_aabb_max_size ) ) return;

        #ifdef G2LIB_USE_CXX11
        vector<shared_ptr<BBox const> > bboxes;
        #else
        vector<BBox const *> bboxes;
        #endif

        bbTriangles_ISO( offs, m_aabb_tri, max_angle, max_size );
        bboxes.reserve(m_aabb_tri.size());
        vector<Triangle2D>::const_iterator it;
        int_type ipos = 0;
        for ( it = m_aabb_tri.begin(); it != m_aabb_tri.end(); ++it, ++ipos ) {
          real_type xmin, ymin, xmax, ymax;
          it->bbox( xmin, ymin, xmax, ymax );
          #ifdef G2LIB_USE_CXX11
          bboxes.push_back( make_shared<BBox const>(
            xmin, ymin, xmax, ymax, G2LIB_CLOTHOID, ipos
          ) );
          #else
          bboxes.push_back(
            new BBox( xmin, ymin, xmax, ymax, G2LIB_CLOTHOID, ipos )
          );
          #endif
        }
        m_aabb_tree.build(bboxes);
        m_aabb_done      = true;
        m_aabb_offs      = offs;
        m_aabb_max_angle = max_angle;
        m_aabb_max_size  = max_size;
      }
      #endif

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

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      bool
      ClothoidList::collision( ClothoidList const & C ) const {
        this->build_AABBtree_ISO( 0 );
        C.build_AABBtree_ISO( 0 );
        T2D_collision_list_ISO fun( this, 0, &C, 0 );
        return m_aabb_tree.collision( C.m_aabb_tree, fun, false );
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      bool
      ClothoidList::collision_ISO(
        real_type            offs,
        ClothoidList const & C,
        real_type            offs_C
      ) const {
        this->build_AABBtree_ISO( offs );
        C.build_AABBtree_ISO( offs_C );
        T2D_collision_list_ISO fun( this, offs, &C, offs_C );
        return m_aabb_tree.collision( C.m_aabb_tree, fun, false );
      }

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

      void
      ClothoidList::intersect_ISO(
        real_type            offs,
        ClothoidList const & CL,
        real_type            offs_CL,
        IntersectList      & ilist,
        bool                 swap_s_vals
      ) const {
        if ( intersect_with_AABBtree ) {
          this->build_AABBtree_ISO( offs );
          CL.build_AABBtree_ISO( offs_CL );
          AABBtree::VecPairPtrBBox iList;
          m_aabb_tree.intersect( CL.m_aabb_tree, iList );

          AABBtree::VecPairPtrBBox::const_iterator ip;
          for ( ip = iList.begin(); ip != iList.end(); ++ip ) {
            size_t ipos1 = size_t(ip->first->Ipos());
            size_t ipos2 = size_t(ip->second->Ipos());

            Triangle2D const & T1 = m_aabb_tri[ipos1];
            Triangle2D const & T2 = CL.m_aabb_tri[ipos2];

            ClothoidCurve const & C1 = m_clotoidList[T1.Icurve()];
            ClothoidCurve const & C2 = CL.m_clotoidList[T2.Icurve()];

            real_type ss1, ss2;
            bool converged = C1.aabb_intersect_ISO( T1, offs, &C2, T2, offs_CL, ss1, ss2 );

            if ( converged ) {
              ss1 += m_s0[T1.Icurve()];
              ss2 += CL.m_s0[T2.Icurve()];
              if ( swap_s_vals ) swap( ss1, ss2 );
              ilist.push_back( Ipair( ss1, ss2 ) );
            }
          }
        } else {
          bbTriangles_ISO( offs, m_aabb_tri, Utils::m_pi/18, 1e100 );
          CL.bbTriangles_ISO( offs_CL, CL.m_aabb_tri, Utils::m_pi/18, 1e100 );
          vector<Triangle2D>::const_iterator i1, i2;
          for ( i1 = m_aabb_tri.begin(); i1 != m_aabb_tri.end(); ++i1 ) {
            for ( i2 = CL.m_aabb_tri.begin(); i2 != CL.m_aabb_tri.end(); ++i2 ) {
              Triangle2D const & T1 = *i1;
              Triangle2D const & T2 = *i2;

              ClothoidCurve const & C1 = m_clotoidList[T1.Icurve()];
              ClothoidCurve const & C2 = CL.m_clotoidList[T2.Icurve()];

              real_type ss1, ss2;
              bool converged = C1.aabb_intersect_ISO( T1, offs, &C2, T2, offs_CL, ss1, ss2 );

              if ( converged ) {
                ss1 += m_s0[T1.Icurve()];
                ss2 += CL.m_s0[T2.Icurve()];
                if ( swap_s_vals ) swap( ss1, ss2 );
                ilist.push_back( Ipair( ss1, ss2 ) );
              }
            }
          }

        }
      }

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

      int_type
      ClothoidList::closestPoint_internal(
        real_type   qx,
        real_type   qy,
        real_type   offs,
        real_type & x,
        real_type & y,
        real_type & s,
        real_type & DST
      ) const {

        this->build_AABBtree_ISO( offs );

        AABBtree::VecPtrBBox candidateList;
        m_aabb_tree.min_distance( qx, qy, candidateList );
        AABBtree::VecPtrBBox::const_iterator ic;
        UTILS_ASSERT0(
          candidateList.size() > 0, "ClothoidList::closestPoint no candidate\n"
        );
        int_type icurve = 0;
        DST = numeric_limits<real_type>::infinity();
        for ( ic = candidateList.begin(); ic != candidateList.end(); ++ic ) {
          size_t ipos = size_t((*ic)->Ipos());
          Triangle2D const & T = m_aabb_tri[ipos];
          real_type dst = T.distMin( qx, qy );
          if ( dst < DST ) {
            // refine distance
            real_type xx, yy, ss;
            m_clotoidList[T.Icurve()].closestPoint_internal(
              T.S0(), T.S1(), qx, qy, offs, xx, yy, ss, dst
            );
            if ( dst < DST ) {
              DST    = dst;
              s      = ss + m_s0[T.Icurve()];
              x      = xx;
              y      = yy;
              icurve = T.Icurve();
            }
          }
        }
        return icurve;
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      int_type
      ClothoidList::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 {

        int_type icurve = this->closestPoint_internal( qx, qy, offs, x, y, s, DST );

        // check if projection is orthogonal
        real_type nx, ny;
        m_clotoidList[icurve].nor_ISO( s - m_s0[icurve], nx, ny );
        real_type qxx = qx - x;
        real_type qyy = qy - y;
        t = qxx * nx + qyy * ny - offs; // signed distance
        real_type pt = abs(qxx * ny - qyy * nx);
        G2LIB_DEBUG_MESSAGE(
          "ClothoidList::closestPoint_ISO\n"
          "||P-P0|| = {} and {}, |(P-P0).T| = {}\n",
          DST, hypot(qxx,qyy), pt
        );
        return pt > GLIB2_TOL_ANGLE*hypot(qxx,qyy) ? -(icurve+1) : icurve;
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      int_type
      ClothoidList::closestPoint_ISO(
        real_type   qx,
        real_type   qy,
        real_type & x,
        real_type & y,
        real_type & s,
        real_type & t,
        real_type & dst
      ) const {
        return closestPoint_ISO( qx, qy, 0, x, y, s, t, dst );
      }

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

      int_type
      ClothoidList::closestSegment( real_type qx, real_type qy ) const {
        this->build_AABBtree_ISO( 0 );

        AABBtree::VecPtrBBox candidateList;
        m_aabb_tree.min_distance( qx, qy, candidateList );
        AABBtree::VecPtrBBox::const_iterator ic;
        UTILS_ASSERT0(
          candidateList.size() > 0, "ClothoidList::closestSegment no candidate\n"
        );
        int_type icurve = 0;
        real_type DST = numeric_limits<real_type>::infinity();
        for ( ic = candidateList.begin(); ic != candidateList.end(); ++ic ) {
          size_t ipos = size_t((*ic)->Ipos());
          Triangle2D const & T = m_aabb_tri[ipos];
          real_type dst = T.distMin( qx, qy );
          if ( dst < DST ) {
            // refine distance
            real_type xx, yy, ss;
            m_clotoidList[T.Icurve()].closestPoint_internal(
              T.S0(), T.S1(), qx, qy, 0, xx, yy, ss, dst
            );
            if ( dst < DST ) {
              DST    = dst;
              icurve = T.Icurve();
            }
          }
        }
        return icurve;
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      int_type
      ClothoidList::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 {
        UTILS_ASSERT0(
          !m_clotoidList.empty(),
          "ClothoidList::closestPointInRange_ISO, empty list\n"
        );
        int_type nsegs = this->numSegments();
        if ( nsegs == 1 ) { // only 1 segment to check
          icurve = 0;
          int_type res = m_clotoidList.front().closestPoint_ISO( qx, qy, x, y, s, t, dst );
          s += m_s0[0];
          return res;
        }

        int_type ib = icurve_begin % nsegs; // to avoid infinite loop in case of bad input
        int_type ie = icurve_end   % nsegs; // to avoid infinite loop in case of bad input
        if ( ib < 0 ) ib += nsegs;
        if ( ie < 0 ) ie += nsegs;
        UTILS_ASSERT(
          ib >= 0 && ie >= 0,
          "ClothoidList::closestPointInRange_ISO, ib = {} ie = {}\n",
          ib, ie
        );

        icurve = ib;
        int_type res = m_clotoidList[icurve].closestPoint_ISO( qx, qy, x, y, s, t, dst );
        s += m_s0[icurve];

        G2LIB_DEBUG_MESSAGE(
          "ClothoidList::closestPointInRange_ISO\n"
          "first segment #{} dst = {} res = {}\n",
          icurve, dst, res
        );

        if ( ib == ie ) return res; // only one segment to check

        int_type iseg = ib;
        do {
          if ( ++iseg >= nsegs ) iseg -= nsegs; // next segment
          real_type C_x, C_y, C_s, C_t, C_dst;
          int_type C_res = m_clotoidList[iseg].closestPoint_ISO(
            qx, qy, C_x, C_y, C_s, C_t, C_dst
          );
          G2LIB_DEBUG_MESSAGE(
            "ClothoidList::closestPointInRange_ISO: segment #{} dst = {} res = {}\n",
            iseg, C_dst, C_res
          );
          if ( C_dst < dst ) {
            dst    = C_dst;
            x      = C_x;
            y      = C_y;
            s      = C_s + m_s0[iseg];
            t      = C_t;
            icurve = iseg;
            res    = C_res;
            G2LIB_DEBUG_MESSAGE(
              "ClothoidList::closestPointInRange_ISO, new min at s = {}, res = {}\n", s, res
            );
          }
        } while ( iseg != ie );
        return res;
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      int_type
      ClothoidList::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 {
        UTILS_ASSERT0(
          !m_clotoidList.empty(),
          "ClothoidList::closestPointInSRange_ISO, empty list\n"
        );
        // put in range
        while ( s_begin < 0              ) s_begin += this->length();
        while ( s_begin > this->length() ) s_begin -= this->length();
        while ( s_end   < 0              ) s_end   += this->length();
        while ( s_end   > this->length() ) s_end   -= this->length();

        // get initial and final segment
        int_type i_begin = findAtS( s_begin );
        int_type i_end   = findAtS( s_end );
        int_type res     = 0;
        if ( i_begin == i_end ) {
          // stesso segmento
          real_type     ss0 = m_s0[i_begin];
          ClothoidCurve C   = m_clotoidList[i_begin]; // crea copia
          C.trim( s_begin-ss0, s_end-ss0 );
          res = C.closestPoint_ISO( qx, qy, x, y, s, t, dst );
          s += s_begin;
        } else {
          // segmenti consecutivi
          int_type  res1;
          real_type x1, y1, s1, t1, dst1;

          real_type     ss0 = m_s0[i_begin];
          real_type     ss1 = m_s0[i_end];
          ClothoidCurve C0  = m_clotoidList[i_begin]; // crea copia
          ClothoidCurve C1  = m_clotoidList[i_end];   // crea copia

          // taglia il segmento
          C0.trim( s_begin-ss0, C0.length() );

          // calcolo closest point
          res = C0.closestPoint_ISO( qx, qy, x, y, s, t, dst );
          s  += s_begin;
          icurve = i_begin;

          G2LIB_DEBUG_MESSAGE(
            "ClothoidList::closestPointInSRange_ISO: first segment {} dst = {} res = {}\n",
            i_begin, dst, res
          );

          C1.trim( 0, s_end-ss1 );
          res1 = C1.closestPoint_ISO( qx, qy, x1, y1, s1, t1, dst1 );
          s1 += ss1;

          G2LIB_DEBUG_MESSAGE(
            "ClothoidList::closestPointInSRange_ISO: last segment {} dst = {} res = {}\n",
            i_end, dst1, res1
          );

          if ( dst1 < dst ) {
            x = x1; y = y1; s = s1; t = t1;
            dst = dst1; res = res1; icurve = i_end;
          }

          // ci sono altri segmenti?
          if ( i_end < i_begin ) i_end += int_type(m_clotoidList.size());
          ++i_begin;
          if ( i_begin < i_end ) {
            int_type icurve1;
            res1 = this->closestPointInRange_ISO(
              qx, qy, i_begin, i_end, x1, y1, s1, t1, dst1, icurve1
            );
            G2LIB_DEBUG_MESSAGE(
              "ClothoidList::closestPointInSRange_ISO: range [{},{}] dst = {} res = {}\n",
              i_begin, i_end, dst1, res1
            );
            if ( dst1 < dst ) {
              x      = x1;
              y      = y1;
              s      = s1;
              t      = t1;
              dst    = dst1;
              res    = res1;
              icurve = icurve1;
              G2LIB_DEBUG_MESSAGE(
                "ClothoidList::closestPointInSRange_ISO: new min at s = {}, res = {}\n", s, res
              );
            }
          }
        }
        return res;
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::getSK( real_type * s, real_type * kappa ) const {
        vector<ClothoidCurve>::const_iterator ic = m_clotoidList.begin();
        int_type  k  = 0;
        real_type ss = 0;
        while ( ic != m_clotoidList.end() ) {
          s[k]     = ss;
          kappa[k] = ic->kappaBegin();
          ss      += ic->length();
          ++k;
          ++ic;
        }
        --ic; // last element
        s[k]     = ss;
        kappa[k] = ic->kappaEnd();
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::getSTK(
        real_type * s,
        real_type * theta,
        real_type * kappa
      ) const {
        vector<ClothoidCurve>::const_iterator ic = m_clotoidList.begin();
        int_type  k  = 0;
        real_type ss = 0;
        while ( ic != m_clotoidList.end() ) {
          s[k]     = ss;
          theta[k] = ic->thetaBegin();
          kappa[k] = ic->kappaBegin();
          ss      += ic->length();
          ++k;
          ++ic;
        }
        --ic; // last element
        s[k]     = ss;
        theta[k] = ic->thetaEnd();
        kappa[k] = ic->kappaEnd();
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::getXY( real_type * x, real_type * y ) const {
        vector<ClothoidCurve>::const_iterator ic = m_clotoidList.begin();
        int_type k  = 0;
        while ( ic != m_clotoidList.end() ) {
          x[k] = ic->xBegin();
          y[k] = ic->yBegin();
          ++k; ++ic;
        }
        --ic;
        x[k] = ic->xEnd();
        y[k] = ic->yEnd();
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::getDeltaTheta( real_type * deltaTheta ) const {
        vector<ClothoidCurve>::const_iterator ic = m_clotoidList.begin();
        int_type k = 0;
        for ( ++ic; ic != m_clotoidList.end(); ++ic, ++k ) {
          real_type tmp = ic->thetaBegin()-ic[-1].thetaEnd();
          if      ( tmp >  Utils::m_pi ) tmp -= Utils::m_2pi;
          else if ( tmp < -Utils::m_pi ) tmp += Utils::m_2pi;
          deltaTheta[k] = tmp;
        }
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::getDeltaKappa( real_type * deltaKappa ) const {
        vector<ClothoidCurve>::const_iterator ic = m_clotoidList.begin();
        int_type k = 0;
        for ( ++ic; ic != m_clotoidList.end(); ++ic, ++k  )
          deltaKappa[k] = ic->kappaBegin()-ic[-1].kappaEnd();

      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      int_type
      ClothoidList::findST1(
        real_type   x,
        real_type   y,
        real_type & s,
        real_type & t
      ) const {

        UTILS_ASSERT0( !m_clotoidList.empty(), "ClothoidList::findST, empty list\n" );
        vector<ClothoidCurve>::const_iterator ic = m_clotoidList.begin();
        vector<real_type>::const_iterator     is = m_s0.begin();

        s = t = 0;
        int_type  ipos = 0;
        int_type  iseg = 0;
        real_type S, T;
        bool ok = ic->findST_ISO( x, y, S, T );
        if ( ok ) {
          s    = *is + S;
          t    = T;
          iseg = 0;
        }

        for ( ++ic, ++is, ++ipos;
              ic != m_clotoidList.end();
              ++ic, ++is, ++ipos ) {
          bool ok1 = ic->findST_ISO( x, y, S, T );
          if ( ok && ok1 ) ok1 = abs(T) < abs(t);
          if ( ok1 ) {
            ok   = true;
            s    = *is + S;
            t    = T;
            iseg = ipos;
          }
        }

        return ok ? iseg : -(1+iseg);
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      int_type
      ClothoidList::findST1(
        int_type    ibegin,
        int_type    iend,
        real_type   x,
        real_type   y,
        real_type & s,
        real_type & t
      ) const {

        UTILS_ASSERT0(
          !m_clotoidList.empty(),
          "ClothoidList::findST, empty list\n"
        );
        UTILS_ASSERT(
          ibegin >= 0 && ibegin <= iend && iend < int_type(m_clotoidList.size()),
          "ClothoidList::findST( ibegin={}, iend={}, x, y, s, t ) bad range not in [0,{}]\n",
          ibegin, iend, m_clotoidList.size()-1
        );
        s = t = 0;
        int_type iseg = 0;
        bool ok = false;
        for ( int_type k = ibegin; k <= iend; ++k ) {
          ClothoidCurve const & ck = m_clotoidList[k];
          real_type S, T;
          bool ok1 = ck.findST_ISO( x, y, S, T );
          if ( ok && ok1 ) ok1 = abs(T) < abs(t);
          if ( ok1 ) {
            ok   = true;
            s    = m_s0[k] + S;
            t    = T;
            iseg = k;
          }
        }
        return ok ? iseg : -(1+iseg);
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::export_table( ostream_type & stream ) const {
        stream << "x\ty\ttheta0\tkappa0\tdkappa\tL\n";
        vector<ClothoidCurve>::const_iterator ic = m_clotoidList.begin();
        for (; ic != m_clotoidList.end(); ++ic )
          stream
            << ic->xBegin()     << '\t'
            << ic->yBegin()     << '\t'
            << ic->thetaBegin() << '\t'
            << ic->kappaBegin() << '\t'
            << ic->dkappa()     << '\t'
            << ic->length()     << '\n';
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::export_ruby( ostream_type & stream ) const {
        stream << "data = {\n";
        vector<ClothoidCurve>::const_iterator ic = m_clotoidList.begin();
        for (; ic != m_clotoidList.end(); ++ic )
          stream
            << ic->xBegin()     << '\t'
            << ic->yBegin()     << '\t'
            << ic->thetaBegin() << '\t'
            << ic->kappaBegin() << '\t'
            << ic->dkappa()     << '\t'
            << ic->length()     << '\n';
        stream << "}\n";
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      ostream_type &
      operator << ( ostream_type & stream, ClothoidList const & CL ) {
        vector<ClothoidCurve>::const_iterator ic = CL.m_clotoidList.begin();
        for (; ic != CL.m_clotoidList.end(); ++ic )
          stream << *ic << '\n';
        return stream;
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      #ifndef DOXYGEN_SHOULD_SKIP_THIS
      static
      void
      save_segment( ostream_type & stream, ClothoidCurve const & c ) {
        fmt::print( stream,
          "{:<24}\t{:<24}\t{:<24}\t{:<24}\n"
          "{:<24}\t{:<24}\t{:<24}\t{:<24}\n",
          //------------------
          fmt::format("{:.20}",c.xBegin()),
          fmt::format("{:.20}",c.yBegin()),
          fmt::format("{:.20}",c.thetaBegin()),
          fmt::format("{:.20}",c.kappaBegin()),
          //------------------
          fmt::format("{:.20}",c.xEnd()),
          fmt::format("{:.20}",c.yEnd()),
          fmt::format("{:.20}",c.thetaEnd()),
          fmt::format("{:.20}",c.kappaEnd())
        );
      }
      #endif

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      #ifndef DOXYGEN_SHOULD_SKIP_THIS
      static
      bool
      load_segment(
        istream_type  & stream,
        ClothoidCurve & c,
        real_type       epsi
      ) {
        string line1, line2;
        while ( stream.good() ) {
          if ( !getline(stream,line1) ) return false;
          if ( line1[0] != '#' ) break;
        }
        if ( !stream.good() ) return false;
        while ( stream.good() ) {
          if ( !getline(stream,line2) ) return false;
          if ( line2[0] != '#' ) break;
        }
        if ( !stream.good() ) return false;
        std::istringstream iss1(line1);
        std::istringstream iss2(line2);
        real_type x0, y0, x1, y1, theta0, theta1, kappa0, kappa1;
        iss1 >> x0 >> y0 >> theta0 >> kappa0;
        iss2 >> x1 >> y1 >> theta1 >> kappa1;
        c.build_G1( x0, y0, theta0, x1, y1, theta1 );
        // check segment
        real_type err1 = std::abs( kappa0 - c.kappaBegin() ) * c.length();
        real_type err2 = std::abs( kappa1 - c.kappaEnd() ) * c.length();
        UTILS_ASSERT(
          err1 < epsi && err2 < epsi,
          "load_segment, failed tolerance on curvature\n"
          "begin error = {}, end error = {}\n",
          err1, err2
        );
        return true;
      }
      #endif

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::save( ostream_type & stream ) const {
        vector<ClothoidCurve>::const_iterator ic = m_clotoidList.begin();
        stream << "# x y theta kappa\n";
        for ( int_type nseg = 1; ic != m_clotoidList.end(); ++ic, ++nseg ) {
          stream << "# segment n." << nseg << '\n';
          save_segment( stream, *ic );
        }
        stream << "# EOF\n";
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      ClothoidList::load( istream_type & stream, real_type epsi ) {
        this->init();
        while ( stream.good() ) {
          ClothoidCurve c;
          bool ok = load_segment( stream, c, epsi );
          if ( !ok ) break;
          this->push_back( c );
        }
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      G2solveCLC::save( ostream_type & stream ) const {
        stream << "# x y theta kappa\n";
        save_segment( stream, S0 );
        save_segment( stream, SM );
        save_segment( stream, S1 );
        stream << "# EOF\n";
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

      void
      G2solve3arc::save( ostream_type & stream ) const {
        stream << "# x y theta kappa\n";
        save_segment( stream, S0 );
        save_segment( stream, SM );
        save_segment( stream, S1 );
        stream << "# EOF\n";
      }

      // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

    }

    // EOF: ClothoidList.cc