Program Listing for File PolyLine.hxx

↰ Return to documentation for file (Clothoids/PolyLine.hxx)

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


namespace G2lib {

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

  class CircleArc;
  class Biarc;
  class BiarcList;
  class ClothoidCurve;
  class ClothoidList;

  class PolyLine : public BaseCurve {
    friend class ClothoidList;
    friend class BiarcList;
  private:
    vector<LineSegment> m_polylineList;
    vector<real_type>   m_s0;
    real_type           m_xe;
    real_type           m_ye;

    mutable Utils::BinarySearch<int_type> m_lastInterval;

    mutable bool     m_aabb_done;
    mutable AABBtree m_aabb_tree;

    #ifndef DOXYGEN_SHOULD_SKIP_THIS
    class Collision_list {
      PolyLine const * pPL1;
      PolyLine const * pPL2;
    public:
      Collision_list( PolyLine const * _pPL1, PolyLine const * _pPL2 )
      : pPL1(_pPL1)
      , pPL2(_pPL2)
      {}

      bool
      operator () ( BBox::PtrBBox ptr1, BBox::PtrBBox ptr2 ) const {
        LineSegment const & LS1 = pPL1->m_polylineList[size_t(ptr1->Ipos())];
        LineSegment const & LS2 = pPL2->m_polylineList[size_t(ptr2->Ipos())];
        return LS1.collision( LS2 );
      }
    };
    #endif

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

  public:

    //explicit
    PolyLine()
    : BaseCurve(G2LIB_POLYLINE)
    , m_aabb_done(false)
    { this->resetLastInterval(); }

    void
    init();

    void
    copy( PolyLine const & l );

    //explicit
    PolyLine( PolyLine const & PL )
    : BaseCurve(G2LIB_POLYLINE)
    , m_aabb_done(false)
    { this->resetLastInterval(); copy(PL); }

    int_type
    findAtS( real_type & s ) const;

    explicit PolyLine( LineSegment const & LS );
    explicit PolyLine( CircleArc const & C, real_type tol );
    explicit PolyLine( Biarc const & B, real_type tol );
    explicit PolyLine( ClothoidCurve const & B, real_type tol );
    explicit PolyLine( ClothoidList const & B, real_type tol );
    explicit PolyLine( BaseCurve const & C );

    PolyLine const & operator = ( PolyLine const & s )
    { copy(s); return *this; }

    LineSegment const &
    getSegment( int_type n ) const;

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

    int_type
    numPoints() const
    { return int_type(m_s0.size()); }

    void polygon( real_type * x, real_type * y) const;
    void init( real_type x0, real_type y0 );
    void push_back( real_type x, real_type y );
    void push_back( LineSegment const & C );
    void push_back( CircleArc const & C, real_type tol );
    void push_back( Biarc const & C, real_type tol );
    void push_back( ClothoidCurve const & C, real_type tol );
    void push_back( ClothoidList const & L, real_type tol );

    void
    build(
      real_type const * x,
      real_type const * y,
      int_type npts
    );

    void build( LineSegment const & L );
    void build( CircleArc const & C, real_type tol );
    void build( Biarc const & B, real_type tol );
    void build( ClothoidCurve const & C, real_type tol );
    void build( ClothoidList const & CL, real_type tol );

    void
    bbox(
      real_type & xmin,
      real_type & ymin,
      real_type & xmax,
      real_type & ymax
    ) const override;

    void
    bbox_ISO(
      real_type   /* offs */,
      real_type & /* xmin */,
      real_type & /* ymin */,
      real_type & /* xmax */,
      real_type & /* ymax */
    ) const override {
      UTILS_ERROR0( "PolyLine::bbox( offs ... ) not available!\n" );
    }

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

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

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

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

    real_type
    length() const override
    { return m_s0.back(); }

    real_type
    length_ISO( real_type ) const override {
      UTILS_ERROR0( "PolyLine::length( offs ) not available!\n" );
      return 0;
    }

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

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

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

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

    real_type
    X( real_type s ) const override {
      int_type idx = this->findAtS( s );
      real_type ss = m_s0[size_t(idx)];
      return m_polylineList[size_t(idx)].X(s-ss);
    }

    real_type
    X_D( real_type s ) const override {
      int_type idx = this->findAtS( s );
      return m_polylineList[size_t(idx)].m_c0;
    }

    real_type
    X_DD( real_type ) const override
    { return 0; }

    real_type
    X_DDD( real_type ) const override
    { return 0; }

    real_type
    Y( real_type s ) const override {
      int_type idx = this->findAtS( s );
      real_type ss = m_s0[size_t(idx)];
      return m_polylineList[size_t(idx)].Y(s-ss);
    }

    real_type
    Y_D( real_type s ) const override {
      int_type idx = this->findAtS( s );
      return m_polylineList[size_t(idx)].m_s0;
    }

    real_type
    Y_DD( real_type ) const override
    { return 0; }

    real_type
    Y_DDD( real_type ) const override
    { return 0; }

    real_type
    theta( real_type s ) const override;

    real_type
    theta_D( real_type s ) const override;

    real_type
    theta_DD( real_type s ) const override;

    real_type
    theta_DDD( real_type s ) const override;

    void
    eval(
      real_type   s,
      real_type & x,
      real_type & y
    ) const override {
      int_type idx = this->findAtS( s );
      real_type ss = m_s0[size_t(idx)];
      m_polylineList[size_t(idx)].eval( s-ss, x, y );
    }

    void
    eval_D(
      real_type   s,
      real_type & x_D,
      real_type & y_D
    ) const override {
      int_type idx = this->findAtS( s );
      real_type ss = m_s0[size_t(idx)];
      m_polylineList[size_t(idx)].eval_D( s-ss, x_D, y_D );
    }

    void
    eval_DD(
      real_type,
      real_type & x_DD,
      real_type & y_DD
    ) const override
    { x_DD = y_DD = 0; }

    void
    eval_DDD(
      real_type,
      real_type & x_DDD,
      real_type & y_DDD
    ) const override
    { x_DDD = y_DDD = 0; }

    // ---

    void
    eval_ISO(
      real_type   s,
      real_type   offs,
      real_type & x,
      real_type & y
    ) const override {
      int_type idx = this->findAtS( s );
      real_type ss = m_s0[size_t(idx)];
      m_polylineList[size_t(idx)].eval_ISO( s-ss, offs, x, y );
    }

    void
    eval_ISO_D(
      real_type   s,
      real_type   offs,
      real_type & x_D,
      real_type & y_D
    ) const override {
      int_type idx = this->findAtS( s );
      real_type ss = m_s0[size_t(idx)];
      m_polylineList[size_t(idx)].eval_ISO_D( s-ss, offs, x_D, y_D );
    }

    void
    eval_ISO_DD(
      real_type,
      real_type,
      real_type & x_DD,
      real_type & y_DD
    ) const override
    { x_DD = y_DD = 0; }

    void
    eval_ISO_DDD(
      real_type,
      real_type,
      real_type & x_DDD,
      real_type & y_DDD
    ) const override
    { x_DDD = y_DDD = 0; }

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

    void
    translate( real_type tx, real_type ty ) override {
      std::vector<LineSegment>::iterator il;
      for ( il = m_polylineList.begin(); il != m_polylineList.end(); ++il )
        il->translate( tx, ty );
    }

    void
    rotate(
      real_type angle,
      real_type cx,
      real_type cy
    ) override {
      std::vector<LineSegment>::iterator il;
      for ( il = m_polylineList.begin(); il != m_polylineList.end(); ++il )
        il->rotate( angle, cx, cy );
    }

    void
    reverse() override;

    void
    scale( real_type sc ) override;

    void
    changeOrigin( real_type newx0, real_type newy0 ) override;

    void
    trim( real_type s_begin, real_type s_end ) override;

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

    int_type
    closestPoint_ISO(
      real_type   x,
      real_type   y,
      real_type & X,
      real_type & Y,
      real_type & S,
      real_type & T,
      real_type & DST
    ) const override;

    int_type
    closestPoint_ISO(
      real_type   /* x    */,
      real_type   /* y    */,
      real_type   /* offs */,
      real_type & /* X    */,
      real_type & /* Y    */,
      real_type & /* S    */,
      real_type & /* T    */,
      real_type & /* DST  */
    ) const override {
      UTILS_ERROR( "PolyLine::closestPoint( ... offs ... ) not available!\n" );
    }

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

    bool
    collision( PolyLine const & C ) const;

    bool
    collision_ISO(
      real_type        offs,
      PolyLine const & CL,
      real_type        offs_CL
    ) const {
      UTILS_ASSERT0(
        Utils::isZero(offs) && Utils::isZero(offs_CL),
        "PolyLine::collision( offs ... ) not available!\n"
      );
      return this->collision( CL );
    }

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

    void
    intersect(
      PolyLine const    & pl,
      vector<real_type> & ss0,
      vector<real_type> & ss1
    ) const;

    void
    intersect(
      PolyLine const & pl,
      IntersectList  & ilist,
      bool             swap_s_vals
    ) const;

    void
    intersect_ISO(
      real_type        offs,
      PolyLine const & pl,
      real_type        offs_pl,
      IntersectList  & ilist,
      bool             swap_s_vals
    ) {
      UTILS_ASSERT0(
        Utils::isZero(offs) && Utils::isZero(offs_pl),
        "PolyLine::intersect( offs ... ) not available!\n"
      );
      this->intersect( pl, ilist, swap_s_vals );
    }

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

    friend
    ostream_type &
    operator << ( ostream_type & stream, PolyLine const & P );

    void
    build_AABBtree( AABBtree & aabb ) const;

    void
    build_AABBtree() const {
      if ( !m_aabb_done ) {
        this->build_AABBtree( m_aabb_tree );
        m_aabb_done = true;
      }
    }

  };

}