# Program Listing for File PolyLine.cc

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

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

    #include "Clothoids.hh"

    // Workaround for Visual Studio
    #ifdef min
      #undef min
    #endif

    #ifdef max
      #undef max
    #endif

    #include <algorithm>

    namespace G2lib {

      using std::min;
      using std::max;
      using std::abs;
      using std::cout;
      using std::vector;
      using std::ceil;

      #ifndef DOXYGEN_SHOULD_SKIP_THIS
      typedef vector<LineSegment>::difference_type LS_dist_type;
      #endif

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

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

      PolyLine::PolyLine( BaseCurve const & C )
      : BaseCurve(G2LIB_POLYLINE)
      , m_aabb_done(false)
      {
        this->resetLastInterval();
        switch ( C.type() ) {
        case G2LIB_LINE:
          build( *static_cast<LineSegment const *>(&C) );
          break;
        case G2LIB_POLYLINE:
          copy( *static_cast<PolyLine const *>(&C) );
          break;
        case G2LIB_CIRCLE:
        case G2LIB_CLOTHOID:
        case G2LIB_BIARC:
        case G2LIB_BIARC_LIST:
        case G2LIB_CLOTHOID_LIST:
          UTILS_ERROR(
            "PolyLine constructor cannot convert from: {}\n",
            CurveType_name[C.type()]
          );
        }
      }

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

      PolyLine::PolyLine( LineSegment const & LS )
      : BaseCurve(G2LIB_POLYLINE)
      , m_aabb_done(false)
      {
        this->resetLastInterval();
        this->init( LS.xBegin(), LS.xBegin() );
        this->push_back( LS );
      }

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

      PolyLine::PolyLine( CircleArc const & C, real_type tol )
      : BaseCurve(G2LIB_POLYLINE)
      , m_aabb_done(false)
      {
        this->resetLastInterval();
        this->init( C.xBegin(), C.xBegin() );
        this->push_back( C, tol );
      }

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

      PolyLine::PolyLine( Biarc const & B, real_type tol )
      : BaseCurve(G2LIB_POLYLINE)
      , m_aabb_done(false)
      {
        this->resetLastInterval();
        this->init( B.xBegin(), B.xBegin() );
        this->push_back( B, tol );
      }

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

      PolyLine::PolyLine( ClothoidCurve const & C, real_type tol )
      : BaseCurve(G2LIB_POLYLINE)
      , m_aabb_done(false)
      {
        this->resetLastInterval();
        this->init( C.xBegin(), C.xBegin() );
        this->push_back( C, tol );
      }

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

      PolyLine::PolyLine( ClothoidList const & PL, real_type tol )
      : BaseCurve(G2LIB_POLYLINE)
      , m_aabb_done(false)
      {
        this->resetLastInterval();
        this->init( PL.xBegin(), PL.xBegin() );
        this->push_back( PL, tol );
      }

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

      int_type
      PolyLine::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, false, true
        );
        return lastInterval;
      }

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

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

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

      void
      PolyLine::copy( PolyLine const & PL ) {
        this->init();
        m_polylineList.reserve( PL.m_polylineList.size() );
        std::copy(
          PL.m_polylineList.begin(),
          PL.m_polylineList.end(),
          back_inserter(m_polylineList)
        );
        m_s0.reserve( PL.m_s0.size() );
        std::copy( PL.m_s0.begin(), PL.m_s0.end(), back_inserter(m_s0) );
      }

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

      LineSegment const &
      PolyLine::getSegment( int_type n ) const {
        UTILS_ASSERT0(
          !m_polylineList.empty(),
          "PolyLine::getSegment(...) empty PolyLine\n"
        );
        UTILS_ASSERT(
          n >= 0 && n < int_type(m_polylineList.size()),
          "PolyLine::getSegment( {} ) out of range [0,{}]\n",
          n, m_polylineList.size()-1
        );
        return m_polylineList[size_t(n)];
      }

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

      void
      PolyLine::polygon( real_type * x, real_type * y) const {
        int_type n = int_type(m_polylineList.size());
        for ( size_t k = 0; k < size_t(n); ++k ) {
          x[k] = m_polylineList[k].xBegin();
          y[k] = m_polylineList[k].yBegin();
        }
        x[size_t(n)] = m_polylineList[size_t(n-1)].xEnd();
        y[size_t(n)] = m_polylineList[size_t(n-1)].yEnd();
      }

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

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

      void
      PolyLine::bbox(
        real_type & xmin,
        real_type & ymin,
        real_type & xmax,
        real_type & ymax
      ) const {

        UTILS_ASSERT0( !m_polylineList.empty(), "PolyLine::bbox, empty list\n" );

        if ( m_aabb_done ) {
          m_aabb_tree.bbox( xmin, ymin, xmax, ymax );
        } else {
          vector<LineSegment>::const_iterator ic = m_polylineList.begin();
          xmin = xmax = ic->xBegin();
          ymin = ymax = ic->yBegin();
          for ( ++ic; ic != m_polylineList.end(); ++ic ) {
            real_type x = ic->xBegin();
            real_type y = ic->yBegin();
            if      ( x < xmin ) xmin = x;
            else if ( x > xmax ) xmax = x;
            if      ( y < ymin ) ymin = y;
            else if ( y > ymax ) ymax = y;
          }
          --ic;
          real_type x = ic->xEnd();
          real_type y = ic->yEnd();
          if      ( x < xmin ) xmin = x;
          else if ( x > xmax ) xmax = x;
          if      ( y < ymin ) ymin = y;
          else if ( y > ymax ) ymax = y;
        }
      }

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

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

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

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

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

      real_type
      PolyLine::theta( real_type s ) const {
        int_type idx = this->findAtS( s );
        return m_polylineList[idx].m_theta0;
      }

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

      real_type
      PolyLine::theta_D( real_type ) const
      { return 0; }

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

      real_type
      PolyLine::theta_DD( real_type ) const
      { return 0; }

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

      real_type
      PolyLine::theta_DDD( real_type ) const
      { return 0; }

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

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

      void
      PolyLine::scale( real_type sfactor ) {
        vector<LineSegment>::iterator ic = m_polylineList.begin();
        real_type newx0 = ic->xBegin();
        real_type newy0 = ic->yBegin();
        m_s0[0] = 0;
        for ( size_t k=0; ic != m_polylineList.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
      PolyLine::reverse() {
        std::reverse( m_polylineList.begin(), m_polylineList.end() );
        vector<LineSegment>::iterator ic = m_polylineList.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_polylineList.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
      PolyLine::changeOrigin( real_type newx0, real_type newy0 ) {
        vector<LineSegment>::iterator ic = m_polylineList.begin();
        for (; ic != m_polylineList.end(); ++ic ) {
          ic->changeOrigin( newx0, newy0 );
          newx0 = ic->xEnd();
          newy0 = ic->yEnd();
        }
      }

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

      void
      PolyLine::trim( real_type s_begin, real_type s_end ) {
        UTILS_ASSERT(
          s_begin >= m_s0.front() && s_end <= m_s0.back() && s_end > s_begin,
          "void::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));
        m_polylineList[i_begin].trim( s_begin-m_s0[i_begin], m_s0[i_begin+1] );
        m_polylineList[i_end].trim( m_s0[i_end], s_end-m_s0[i_end] );
        m_polylineList.erase( m_polylineList.begin()+LS_dist_type(i_end+1), m_polylineList.end() );
        m_polylineList.erase( m_polylineList.begin(), m_polylineList.begin()+LS_dist_type(i_begin) );
        vector<LineSegment>::iterator ic = m_polylineList.begin();
        m_s0[0] = 0;
        size_t k = 0;
        for (; ic != m_polylineList.end(); ++ic, ++k )
          m_s0[k+1] = m_s0[k] + ic->length();
        this->resetLastInterval();
      }

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

      void
      PolyLine::trim( real_type s_begin, real_type s_end, PolyLine & newPL ) const {

        newPL.init();

        if ( m_polylineList.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_polylineList.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];
            LineSegment LL  = m_polylineList[i_begin];
            LL.trim( s_begin-ss0, s_end-ss0 );
            newPL.push_back( LL );
          } else {
            LineSegment L0 = m_polylineList[i_begin];
            L0.trim( s_begin - m_s0[i_begin], L0.length() );
            newPL.push_back( L0 );

            for ( ++i_begin; i_begin < i_end; ++i_begin )
              newPL.push_back( m_polylineList[i_begin] );

            LineSegment L1 = m_polylineList[i_end];
            L1.trim( 0, s_end - m_s0[i_end] );
            newPL.push_back( L1 );
          }
        } else {
          LineSegment L0 = m_polylineList[i_begin];
          L0.trim( s_begin - m_s0[i_begin], L0.length() );
          newPL.push_back( L0 );

          for ( ++i_begin; i_begin < n_seg; ++i_begin )
            newPL.push_back( m_polylineList[i_begin] );

          for ( i_begin = 0; i_begin < i_end; ++i_begin )
            newPL.push_back( m_polylineList[i_begin] );

          LineSegment L1 = m_polylineList[i_end];
          L1.trim( 0, s_end - m_s0[i_end] );
          newPL.push_back( L1 );
        }
      }

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

      void
      PolyLine::build_AABBtree( AABBtree & aabbtree ) const {
        #ifdef G2LIB_USE_CXX11
        vector<shared_ptr<BBox const> > bboxes;
        #else
        vector<BBox const *> bboxes;
        #endif
        bboxes.reserve(m_polylineList.size());
        vector<LineSegment>::const_iterator it;
        int_type ipos = 0;
        for ( it = m_polylineList.begin(); it != m_polylineList.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_LINE, ipos
          ) );
          #else
          bboxes.push_back( new BBox( xmin, ymin, xmax, ymax, G2LIB_LINE, ipos ) );
          #endif
        }
        aabbtree.build(bboxes);
      }

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

      void
      PolyLine::init( real_type x0, real_type y0 ) {
        m_xe = x0;
        m_ye = y0;
        m_polylineList.clear();
        m_s0.clear();
        m_s0.push_back(0);
        m_aabb_done = false;
      }

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

      void
      PolyLine::push_back( real_type x, real_type y ) {
        LineSegment s;
        s.build_2P( m_xe, m_ye, x, y );
        m_polylineList.push_back( s );
        real_type slast = m_s0.back() + s.length();
        m_s0.push_back( slast );
        m_xe = x;
        m_ye = y;
        m_aabb_done = false;
      }

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

      void
      PolyLine::push_back( LineSegment const & C ) {
        m_polylineList.push_back( C );
        LineSegment & S = m_polylineList.back();
        S.changeOrigin( m_xe, m_ye );
        real_type slast = m_s0.back() + S.length();
        m_s0.push_back( slast );
        m_xe = S.xEnd();
        m_ye = S.yEnd();
        m_aabb_done = false;
      }

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

      void
      PolyLine::push_back( CircleArc const & C, real_type tol ) {
        real_type L  = C.length();
        int_type  ns = int_type(ceil( L / C.lenTolerance( tol ) ));
        real_type tx = m_xe - C.xBegin();
        real_type ty = m_ye - C.yBegin();
        for ( int_type i = 1; i < ns; ++i ) {
          real_type s = (i*L)/ns;
          push_back( tx + C.X(s), ty + C.Y(s) );
        }
        push_back( tx + C.xEnd(), ty + C.yEnd() );
        m_xe = tx + C.xEnd();
        m_ye = ty + C.yEnd();
        m_aabb_done = false;
      }

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

      void
      PolyLine::push_back( Biarc const & B, real_type tol ) {
        CircleArc const & C0 = B.C0();
        CircleArc const & C1 = B.C1();
        real_type L0  = C0.length();
        real_type L1  = C1.length();
        int_type  ns0 = int_type(ceil( L0 / C0.lenTolerance( tol ) ));
        int_type  ns1 = int_type(ceil( L1 / C1.lenTolerance( tol ) ));

        real_type tx = m_xe - C0.xBegin();
        real_type ty = m_ye - C0.yBegin();

        for ( int_type i = 1; i < ns0; ++i ) {
          real_type s = (i*L0)/ns0;
          push_back( tx + C0.X(s), ty + C0.Y(s) );
        }
        push_back( tx + C1.xBegin(), ty + C1.yBegin() );
        for ( int_type i = 1; i < ns1; ++i ) {
          real_type s = (i*L1)/ns1;
          push_back( tx + C1.X(s), ty + C1.Y(s) );
        }
        push_back( tx + C1.xEnd(), ty + C1.yEnd() );
        m_xe = tx + C1.xEnd();
        m_ye = ty + C1.yEnd();
        m_aabb_done = false;
      }

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

      void
      PolyLine::push_back( ClothoidCurve const & C, real_type tol ) {

        real_type L    = C.length();
        real_type absk = max(abs(C.kappaBegin()), abs(C.kappaEnd()));
        real_type tmp  = absk*tol - 1;
        int_type ns = 1;
        if ( tmp > -1 ) ns = int_type( ceil( L*absk/(2*(Utils::m_pi-acos(tmp))) ) );

        real_type tx = m_xe - C.xBegin();
        real_type ty = m_ye - C.yBegin();
        for ( int_type i = 1; i < ns; ++i ) {
          real_type s = (i*L)/ns;
          push_back( tx + C.X(s), ty + C.Y(s) );
        }

        push_back( tx + C.xEnd(), ty + C.yEnd() );
        m_xe = tx + C.xEnd();
        m_ye = ty + C.yEnd();
        m_aabb_done = false;
      }

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

      void
      PolyLine::push_back( ClothoidList const & L, real_type tol ) {
        int_type ns = L.numSegments();
        for ( int_type idx = 0; idx < ns; ++idx ) {
          ClothoidCurve const & C = L.get( idx );
          push_back( C, tol );
        }
        // aabb_done = false;
      }

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

      void
      PolyLine::build(
        real_type const * x,
        real_type const * y,
        int_type          npts
      ) {
        init( x[0], y[0] );
        for ( int_type k = 1; k < npts; ++k )
          push_back( x[k], y[k] );
      }

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

      void
      PolyLine::build( LineSegment const & C ) {
        init( C.xBegin(), C.yBegin() );
        push_back( C.xEnd(), C.yEnd() );
      }

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

      void
      PolyLine::build( CircleArc const & C, real_type tol ) {
        init( C.xBegin(), C.yBegin() );
        push_back( C, tol );
      }

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

      void
      PolyLine::build( Biarc const & C, real_type tol ) {
        init( C.xBegin(), C.yBegin() );
        push_back( C, tol );
      }

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

      void
      PolyLine::build( ClothoidCurve const & C, real_type tol ) {
        init( C.xBegin(), C.yBegin() );
        push_back( C, tol );
      }

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

      void
      PolyLine::build( ClothoidList const & L, real_type tol ) {
        init( L.xBegin(), L.yBegin() );
        push_back( L, tol );
      }

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

      int_type
      PolyLine::closestPoint_ISO(
        real_type   x,
        real_type   y,
        real_type & X,
        real_type & Y,
        real_type & S,
        real_type & T,
        real_type & DST
      ) const{
        UTILS_ASSERT0(
          !m_polylineList.empty(),
          "PolyLine::closestPoint, empty list\n"
        );
        vector<LineSegment>::const_iterator ic = m_polylineList.begin();
        vector<real_type>::const_iterator   is = m_s0.begin();
        ic->closestPoint_ISO( x, y, X, Y, S, T, DST );
        int_type ipos = 0;
        for ( ++ic, ++is; ic != m_polylineList.end(); ++ic, ++is ) {
          real_type X1, Y1, S1, T1, DST1;
          ic->closestPoint_ISO( x, y, X1, Y1, S1, T1, DST1 );
          if ( DST1 < DST ) {
            DST  = DST1;
            X    = X1;
            Y    = Y1;
            S    = *is + S1;
            T    = T1;
            ipos = int_type(ic-m_polylineList.begin());
          }
        }

        real_type xx, yy;
        m_polylineList[size_t(ipos)].eval_ISO( S - m_s0[size_t(ipos)], T, xx, yy );
        real_type err = hypot( x - xx, y - yy );
        real_type tol = (DST > 1 ? DST*machepsi1000 : machepsi1000);
        if ( err > tol ) return -(ipos+1);
        return ipos;
      }

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

      bool
      PolyLine::collision( PolyLine const & C ) const {
        this->build_AABBtree();
        C.build_AABBtree();
        Collision_list fun( this, &C );
        return m_aabb_tree.collision( C.m_aabb_tree, fun, false );
      }

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

      void
      PolyLine::intersect(
        PolyLine const    & pl,
        vector<real_type> & ss0,
        vector<real_type> & ss1
      ) const {
        UTILS_ASSERT0(
          !m_polylineList.empty(),
          "PolyLine::intersect, empty list\n"
        );
        UTILS_ASSERT(
          !pl.m_polylineList.empty(),
          "PolyLine::intersect, empty secondary list\n"
        );

    #if 1
        build_AABBtree();
        pl.build_AABBtree();
        AABBtree::VecPairPtrBBox intersectionList;
        m_aabb_tree.intersect( pl.m_aabb_tree, intersectionList );
        AABBtree::VecPairPtrBBox::const_iterator ip;
        for ( ip = intersectionList.begin(); ip != intersectionList.end(); ++ip ) {
          size_t ipos0 = size_t(ip->first->Ipos());
          size_t ipos1 = size_t(ip->second->Ipos());
          UTILS_ASSERT(
            ipos0 < m_polylineList.size(),
            "Bad ipos0 = {}\n", ipos0
          );
          UTILS_ASSERT(
            ipos1 < pl.m_polylineList.size(),
            "Bad ipos1 = {}\n", ipos1
          );
          real_type sss0, sss1;
          bool ok = m_polylineList[ipos0].intersect(pl.m_polylineList[ipos1],sss0,sss1);
          if ( ok ) {
            ss0.push_back(sss0+m_s0[ipos0]);
            ss1.push_back(sss1+pl.m_s0[ipos1]);
          }
        }

    #else
        ss0.clear();
        ss1.clear();
        vector<LineSegment>::const_iterator ic0 = lvec.begin();
        vector<real_type>::const_iterator   is0 = s0.begin();
        while ( ic0 != lvec.end() ) {
          vector<LineSegment>::const_iterator ic1 = pl.lvec.begin();
          vector<real_type>::const_iterator   is1 = pl.s0.begin();
          while ( ic1 != pl.lvec.end() ) {
            real_type a0, a1;
            bool ok = ic0->intersect( *ic1, a0, a1 );
            if ( ok ) {
              ss0.push_back( (*is0) + a0 );
              ss1.push_back( (*is1) + a1 );
            }
            ++ic1;
            ++is1;
          }
          ++ic0;
          ++is0;
        }
    #endif

      }

      void
      PolyLine::intersect(
        PolyLine const & pl,
        IntersectList  & ilist,
        bool             swap_s_vals
      ) const {
        std::vector<real_type> s1, s2;
        this->intersect( pl, s1, s2 );
        ilist.reserve( ilist.size() + s1.size() );
        for ( size_t i=0; i < s1.size(); ++i ) {
          real_type ss1 = s1[i];
          real_type ss2 = s2[i];
          if ( swap_s_vals ) std::swap( ss1, ss2 );
          ilist.push_back( Ipair(ss1,ss2) );
        }
      }

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

      ostream_type &
      operator << ( ostream_type & stream, PolyLine const & P ) {
        fmt::print( stream,
          "nseg   = {}\n"
          "xBegin = {}\n"
          "ybegin = {}\n"
          "xEnd   = {}\n"
          "yEnd   = {}\n"
          "length = {}\n",
          P.numSegments(),
          P.xBegin(),
          P.yBegin(),
          P.xEnd(),
          P.yEnd(),
          P.length()
        );
        return stream;
      }

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

    }

    // EOF: PolyLine.cc