# Program Listing for File Circle.cc

↰ Return to documentation for file (`Circle.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 <cmath>
    #include <algorithm>

    namespace G2lib {

      using std::min;
      using std::max;
      using std::abs;
      using std::tan;
      using std::abs;
      using std::ceil;
      using std::floor;
      using std::swap;
      using std::vector;

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

      CircleArc::CircleArc( BaseCurve const & C )
      : BaseCurve(G2LIB_CIRCLE)
      {
        switch ( C.type() ) {
        case G2LIB_LINE:
          {
            LineSegment const & LS = *static_cast<LineSegment const *>(&C);
            m_x0     = LS.xBegin();
            m_y0     = LS.yBegin();
            m_theta0 = LS.m_theta0;
            m_c0     = LS.m_c0;
            m_s0     = LS.m_s0;
            m_k      = 0;
            m_L      = LS.length();
          }
          break;
        case G2LIB_CIRCLE:
          *this = *static_cast<CircleArc const *>(&C);
          break;
        case G2LIB_CLOTHOID:
        case G2LIB_BIARC:
        case G2LIB_BIARC_LIST:
        case G2LIB_CLOTHOID_LIST:
        case G2LIB_POLYLINE:
          UTILS_ERROR(
            "CircleArc constructor cannot convert from: {}\n",
            CurveType_name[C.type()]
          );
        }
      }

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

      bool
      CircleArc::build_G1(
        real_type x0,
        real_type y0,
        real_type theta0,
        real_type x1,
        real_type y1
      ) {

        real_type dx = x1 - x0;
        real_type dy = y1 - y0;
        real_type d  = hypot( dx, dy );

        if ( d > 0 ) {
          real_type th = atan2( dy, dx ) - theta0;
          m_x0     = x0;
          m_y0     = y0;
          m_theta0 = theta0;
          m_k      = 2*sin(th)/d;
          m_L      = d/Sinc(th);
          return true;
        }
        return false;
      }

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

      bool
      CircleArc::build_3P(
        real_type _x0,
        real_type _y0,
        real_type _x1,
        real_type _y1,
        real_type _x2,
        real_type _y2
      ) {
        real_type dxa   = _x1 - _x0;
        real_type dya   = _y1 - _y0;
        real_type dxb   = _x2 - _x1;
        real_type dyb   = _y2 - _y1;
        real_type La    = hypot(dya,dxa);
        real_type Lb    = hypot(dyb,dxb);
        real_type cosom = (dxa*dxb + dya*dyb)/(La*Lb);
        if      ( cosom >  1 ) cosom = 1;
        else if ( cosom < -1 ) cosom = -1;
        real_type omega = acos(cosom);

        real_type alpha = omega - atan2(Lb*sin(omega),La+Lb*cos(omega));
        real_type dxc   = _x2 - _x0;
        real_type dyc   = _y2 - _y0;
        real_type Lc    = hypot(dyc,dxc);
        real_type cosal = (dxa*dxc + dya*dyc)/(La*Lc);
        if      ( cosal >  1 ) cosal = 1;
        else if ( cosal < -1 ) cosal = -1;
        alpha += acos(cosal);

        if ( dxa*dyb > dya*dxb ) alpha = -alpha;
        real_type _theta0 = atan2( dyc, dxc ) + alpha;
        return build_G1( _x0, _y0, _theta0, _x2, _y2 );
      }

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

      real_type
      CircleArc::thetaMinMax( real_type & thMin, real_type & thMax ) const  {
        thMin = m_theta0;
        thMax = m_theta0 + m_L * m_k;
        if ( thMax < thMin ) swap( thMin, thMax );
        return thMax-thMin;
      }

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

      real_type
      CircleArc::X( real_type s ) const {
        real_type sk = (s*m_k)/2;
        return m_x0+s*Sinc(sk)*cos(m_theta0+sk);
      }

      real_type
      CircleArc::X_D( real_type s ) const {
        return cos(m_theta0+s*m_k);
      }

      real_type
      CircleArc::X_DD( real_type s ) const {
        return -m_k*sin(m_theta0+s*m_k);
      }

      real_type
      CircleArc::X_DDD( real_type s ) const {
        return -(m_k*m_k)*cos(m_theta0+s*m_k);
      }

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

      real_type
      CircleArc::Y( real_type s ) const {
        real_type sk = (s*m_k)/2;
        return m_y0+s*Sinc(sk)*sin(m_theta0+sk);
      }

      real_type
      CircleArc::Y_D( real_type s ) const {
        return sin(m_theta0+s*m_k);
      }

      real_type
      CircleArc::Y_DD( real_type s ) const {
        return m_k*cos(m_theta0+s*m_k);
      }

      real_type
      CircleArc::Y_DDD( real_type s ) const {
        return -(m_k*m_k)*sin(m_theta0+s*m_k);
      }

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

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

      void
      CircleArc::tg(
        real_type   s,
        real_type & tx,
        real_type & ty
      ) const {
        real_type th = theta(s);
        tx = cos(th);
        ty = sin(th);
      }

      void
      CircleArc::tg_D(
        real_type   s,
        real_type & tx_D,
        real_type & ty_D
      ) const {
        real_type th = theta(s);
        tx_D = -sin(th)*m_k;
        ty_D = cos(th)*m_k;
      }

      void
      CircleArc::tg_DD(
        real_type   s,
        real_type & tx_DD,
        real_type & ty_DD
      ) const {
        real_type th = theta(s);
        real_type k2 = m_k*m_k;
        tx_DD = -cos(th)*k2;
        ty_DD = -sin(th)*k2;
      }

      void
      CircleArc::tg_DDD(
        real_type   s,
        real_type & tx_DDD,
        real_type & ty_DDD
      ) const {
        real_type th = theta(s);
        real_type k3 = m_k*m_k*m_k;
        tx_DDD = sin(th)*k3;
        ty_DDD = -cos(th)*k3;
      }

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

      void
      CircleArc::eval(
        real_type   s,
        real_type & x,
        real_type & y
      ) const {
        real_type sk  = (s*m_k)/2;
        real_type LS  = s*Sinc(sk);
        real_type arg = m_theta0+sk;
        x = m_x0+LS*cos(arg);
        y = m_y0+LS*sin(arg);
      }

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

      void
      CircleArc::eval_D(
        real_type   s,
        real_type & x_D,
        real_type & y_D
      ) const {
        real_type arg = m_theta0+s*m_k;
        x_D = cos(arg);
        y_D = sin(arg);
      }

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

      void
      CircleArc::eval_DD(
        real_type   s,
        real_type & x_DD,
        real_type & y_DD
      ) const {
        real_type arg = m_theta0+s*m_k;
        x_DD = -m_k*sin(arg);
        y_DD = m_k*cos(arg);
      }

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

      void
      CircleArc::eval_DDD(
        real_type   s,
        real_type & x_DDD,
        real_type & y_DDD
      ) const {
        real_type arg = m_theta0+s*m_k;
        real_type k2  = m_k*m_k;
        x_DDD = -k2*cos(arg);
        y_DDD = -k2*sin(arg);
      }

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

      void
      CircleArc::trim( real_type s_begin, real_type s_end ) {
        UTILS_ASSERT(
          s_end > s_begin,
          "CircleArc::trim( begin={}, s_end={} ) s_end must be > s_begin\n",
          s_begin, s_end
        );
        real_type x, y;
        eval( s_begin, x, y );
        m_theta0 += s_begin * m_k;
        m_L  = s_end - s_begin;
        m_x0 = x;
        m_y0 = y;
        m_c0 = cos(m_theta0);
        m_s0 = sin(m_theta0);
      }

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

      void
      CircleArc::rotate( real_type angle, real_type cx, real_type cy ) {
        real_type dx  = m_x0 - cx;
        real_type dy  = m_y0 - cy;
        real_type C   = cos(angle);
        real_type S   = sin(angle);
        real_type ndx = C*dx - S*dy;
        real_type ndy = C*dy + S*dx;
        m_x0      = cx + ndx;
        m_y0      = cy + ndy;
        m_theta0 += angle;
        m_c0      = cos(m_theta0);
        m_s0      = sin(m_theta0);
      }

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

      void
      CircleArc::scale( real_type s ) {
        m_k /= s;
        m_L *= s;
      }

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

      void
      CircleArc::reverse() {
        real_type xx, yy;
        eval( m_L, xx, yy );
        m_theta0 += m_L*m_k+Utils::m_pi;
        while ( m_theta0 >  Utils::m_pi ) m_theta0 -= Utils::m_2pi;
        while ( m_theta0 < -Utils::m_pi ) m_theta0 += Utils::m_2pi;
        m_x0 = xx;
        m_y0 = yy;
        m_c0 = cos(m_theta0);
        m_s0 = sin(m_theta0);
        m_k  = -m_k;
      }

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

      void
      CircleArc::center( real_type & cx, real_type & cy ) const {
        real_type nx = -sin(m_theta0);
        real_type ny = cos(m_theta0);
        cx = m_x0 + nx/m_k;
        cy = m_y0 + ny/m_k;
      }

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

      void
      CircleArc::changeCurvilinearOrigin( real_type new_s0, real_type newL ) {
        real_type new_x0, new_y0;
        eval( new_s0,  new_x0, new_y0 );
        m_x0      = new_x0;
        m_y0      = new_y0;
        m_theta0 += m_k*new_s0;
        m_L       = newL;
      }

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

      bool
      CircleArc::bbTriangle(
        real_type & xx0, real_type & yy0,
        real_type & xx1, real_type & yy1,
        real_type & xx2, real_type & yy2
      ) const {
        real_type dtheta = m_L * m_k;
        bool ok = abs(dtheta) <= Utils::m_pi/3;
        if ( ok ) {
          xx0 = m_x0;
          yy0 = m_y0;
          eval( m_L, xx2, yy2 );
          xx1 = (xx0+xx2)/2;
          yy1 = (yy0+yy2)/2;
          real_type nx = yy0-yy2;
          real_type ny = xx2-xx0;
          real_type tg = tan(dtheta/2)/2;
          xx1 -= nx * tg;
          yy1 -= ny * tg;
        }
        return ok;
      }

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

      bool
      CircleArc::bbTriangle_ISO(
        real_type   offs,
        real_type & xx0, real_type & yy0,
        real_type & xx1, real_type & yy1,
        real_type & xx2, real_type & yy2
      ) const {
        real_type dtheta = m_L * m_k;
        bool ok = abs(dtheta) <= Utils::m_pi/3;
        if ( ok ) {
          eval_ISO( 0,   offs, xx0, yy0 );
          eval_ISO( m_L, offs, xx2, yy2 );
          xx1 = (xx0+xx2)/2;
          yy1 = (yy0+yy2)/2;
          real_type nx = yy0-yy2;
          real_type ny = xx2-xx0;
          real_type tg = tan(dtheta/2)/2;
          xx1 -= nx * tg;
          yy1 -= ny * tg;
        }
        return ok;
      }

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

      void
      CircleArc::bbTriangles(
        vector<Triangle2D> & tvec,
        real_type            max_angle,
        real_type            max_size,
        int_type             icurve
      ) const {
        real_type dtheta = abs( min(m_L,max_size) * m_k);
        int_type  n      = 1;
        if ( dtheta > max_angle ) {
          n       = int_type(ceil( dtheta/max_angle ));
          dtheta /= n;
        }
        real_type tg = tan(dtheta/2)/2;
        if ( m_k < 0 ) tg = -tg;
        tvec.reserve( size_t(n) );
        real_type xx0 = m_x0;
        real_type yy0 = m_y0;
        real_type ds  = m_L/n;
        real_type ss  = ds;
        for ( int_type iter = 0; iter < n; ++iter, ss += ds ) {
          real_type xx2, yy2;
          eval( ss, xx2, yy2 );
          real_type xx1 = (xx0+xx2)/2;
          real_type yy1 = (yy0+yy2)/2;
          real_type nx  = yy0-yy2;
          real_type ny  = xx2-xx0;
          xx1 -= nx * tg;
          yy1 -= ny * tg;
          tvec.push_back( Triangle2D( xx0, yy0, xx1, yy1, xx2, yy2, 0, 0, icurve ) );
          xx0 = xx2;
          yy0 = yy2;
        }
      }

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

      void
      CircleArc::bbTriangles_ISO(
        real_type            offs,
        vector<Triangle2D> & tvec,
        real_type            max_angle,
        real_type            max_size,
        int_type             icurve
      ) const {
        real_type scale  = 1+m_k*offs;
        real_type dtheta = abs( min(m_L,max_size/scale) * m_k );
        int_type  n      = 1;
        if ( dtheta > max_angle ) {
          n       = int_type(ceil( dtheta/max_angle ));
          dtheta /= n;
        }
        tvec.reserve( size_t(n) );
        real_type ds = m_L/n;
        real_type ss = ds;
        real_type tg = scale * tan(dtheta/2)/2;
        if ( m_k < 0 ) tg = -tg;
        real_type xx0, yy0;
        eval_ISO( 0, offs, xx0, yy0 );
        for ( int_type iter = 0; iter < n; ++iter, ss += ds ) {
          real_type xx2, yy2;
          eval_ISO( ss, offs, xx2, yy2 );
          real_type xx1 = (xx0+xx2)/2;
          real_type yy1 = (yy0+yy2)/2;
          real_type nx = yy0-yy2;
          real_type ny = xx2-xx0;
          xx1 -= nx * tg;
          yy1 -= ny * tg;
          tvec.push_back( Triangle2D( xx0, yy0, xx1, yy1, xx2, yy2, 0, 0, icurve ) );
          xx0 = xx2;
          yy0 = yy2;
        }
      }

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

      void
      CircleArc::bbox(
        real_type & xmin,
        real_type & ymin,
        real_type & xmax,
        real_type & ymax
      ) const {
        vector<Triangle2D> tvec;
        this->bbTriangles( tvec, Utils::m_pi/4 );
        tvec[0].bbox( xmin, ymin, xmax, ymax );
        for ( int_type iter = 1; iter < int_type(tvec.size()); ++iter ) {
          real_type xmin1, ymin1, xmax1, ymax1;
          tvec[size_t(iter)].bbox( xmin1, ymin1, xmax1, ymax1 );
          if ( xmin1 < xmin ) xmin = xmin1;
          if ( ymin1 < ymin ) ymin = ymin1;
          if ( xmax1 > xmax ) xmax = xmax1;
          if ( ymax1 > ymax ) ymax = ymax1;
        }
      }

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

      void
      CircleArc::bbox_ISO(
        real_type   offs,
        real_type & xmin,
        real_type & ymin,
        real_type & xmax,
        real_type & ymax
      ) const {
        vector<Triangle2D> tvec;
        this->bbTriangles_ISO( offs, tvec, Utils::m_pi/4 );
        tvec[0].bbox( xmin, ymin, xmax, ymax );
        for ( int_type iter = 1; iter < int_type(tvec.size()); ++iter ) {
          real_type xmin1, ymin1, xmax1, ymax1;
          tvec[size_t(iter)].bbox( xmin1, ymin1, xmax1, ymax1 );
          if ( xmin1 < xmin ) xmin = xmin1;
          if ( ymin1 < ymin ) ymin = ymin1;
          if ( xmax1 > xmax ) xmax = xmax1;
          if ( ymax1 > ymax ) ymax = ymax1;
        }
      }

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

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

      bool
      CircleArc::collision( CircleArc const & C ) const {
        real_type s1[2], s2[2];
        int_type ni = intersectCircleCircle(
          m_x0, m_y0, m_theta0, m_k,
          C.m_x0, C.m_y0, C.m_theta0, C.m_k, s1, s2
        );
        real_type eps1 = machepsi100*m_L;
        real_type eps2 = machepsi100*C.m_L;
        for ( int_type i = 0; i < ni; ++i ) {
          if ( s1[i] >= -eps1 && s1[i] <= m_L+eps1 &&
               s2[i] >= -eps2 && s2[i] <= m_L+eps2 )
            return true;
        }
        return false;
      }

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

      bool
      CircleArc::collision_ISO(
        real_type         offs,
        CircleArc const & C,
        real_type         offs_C
      ) const {
        real_type s1[2], s2[2];
        real_type sc1 = 1+m_k*offs;
        real_type sc2 = 1+C.m_k*offs_C;
        int_type ni = intersectCircleCircle(
          this->X_ISO(0,offs),
          this->Y_ISO(0,offs),
          m_theta0,
          m_k/sc2,
          C.X_ISO(0,offs_C),
          C.Y_ISO(0,offs_C),
          C.m_theta0,
          C.m_k/sc2,
          s1, s2
        );
        real_type eps1 = machepsi100*m_L;
        real_type eps2 = machepsi100*C.m_L;
        for ( int_type i = 0; i < ni; ++i ) {
          real_type ss1 = s1[i]/sc1;
          real_type ss2 = s2[i]/sc2;
          if ( ss1 >= -eps1 && ss1 <= m_L+eps1 &&
               ss2 >= -eps2 && ss2 <= C.m_L+eps2 )
            return true;
        }
        return false;
      }

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

      void
      CircleArc::intersect(
        CircleArc const & C,
        IntersectList   & ilist,
        bool              swap_s_vals
      ) const {
        real_type s1[2], s2[2];
        int_type ni = intersectCircleCircle(
          m_x0, m_y0, m_theta0, m_k,
          C.m_x0, C.m_y0, C.m_theta0, C.m_k, s1, s2
        );
        real_type eps1 = machepsi100*m_L;
        real_type eps2 = machepsi100*C.m_L;
        for ( int_type i = 0; i < ni; ++i ) {
          real_type ss1 = s1[i];
          real_type ss2 = s2[i];
          if ( ss1 >= -eps1 && ss1 <= m_L+eps1 &&
               ss2 >= -eps2 && ss2 <= C.m_L+eps2 ) {
            if ( swap_s_vals ) ilist.push_back( Ipair(ss2,ss1) );
            else               ilist.push_back( Ipair(ss1,ss2) );
          }
        }
      }

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

      void
      CircleArc::intersect_ISO(
        real_type         offs,
        CircleArc const & C,
        real_type         offs_C,
        IntersectList   & ilist,
        bool              swap_s_vals
      ) const {
        real_type s1[2], s2[2];
        real_type sc1 = 1+m_k*offs;
        real_type sc2 = 1+C.m_k*offs_C;
        int_type ni = intersectCircleCircle(
          this->X_ISO(0,offs),
          this->Y_ISO(0,offs),
          m_theta0,
          m_k/sc2,
          C.X_ISO(0,offs_C),
          C.Y_ISO(0,offs_C),
          C.m_theta0,
          C.m_k/sc2,
          s1, s2
        );
        real_type eps1 = machepsi100*m_L;
        real_type eps2 = machepsi100*C.m_L;
        for ( int_type i = 0; i < ni; ++i ) {
          real_type ss1 = s1[i]/sc1;
          real_type ss2 = s2[i]/sc2;
          if ( ss1 >= -eps1 && ss1 <= m_L+eps1 &&
               ss2 >= -eps2 && ss2 <= C.m_L+eps2 ) {
            if ( swap_s_vals ) ilist.push_back( Ipair(ss2,ss1) );
            else               ilist.push_back( Ipair(ss1,ss2) );
          }
        }
      }

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

      int_type
      CircleArc::closestPoint_ISO(
        real_type   qx,
        real_type   qy,
        real_type & x,
        real_type & y,
        real_type & s,
        real_type & t,
        real_type & dst
      ) const {
        real_type cc0 = cos(m_theta0);
        real_type ss0 = sin(m_theta0);
        s = projectPointOnCircleArc( m_x0, m_y0, cc0, ss0, m_k, m_L, qx, qy );
        int_type res = 1;
        if ( s < 0 || s > m_L ) {
          s = m_L;
          t = 0;
          eval( s, x, y );
          // costruisco piano
          real_type nx = x-m_x0;
          real_type ny = y-m_y0;
          real_type dx = 2*qx-(m_x0+x);
          real_type dy = 2*qy-(m_y0+y);
          if ( nx*dx + ny*dy <= 0 ) {
            s = 0;
            x = m_x0;
            y = m_y0;
          }
          res = -1;
        } else {
          eval( s, x, y );
        }
        real_type nx, ny;
        nor_ISO( s, nx, ny );
        real_type dx = qx-x;
        real_type dy = qy-y;
        t   = dx * nx + dy * ny;
        dst = hypot( dx, dy );
        return res;
      }

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

      int_type
      CircleArc::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  {
        real_type cc0 = cos(m_theta0);
        real_type ss0 = sin(m_theta0);
        real_type xx0 = m_x0+offs*nx_Begin_ISO();
        real_type yy0 = m_y0+offs*ny_Begin_ISO();
        real_type ff  = 1+m_k*offs;
        real_type LL  = m_L*ff;
        s = projectPointOnCircleArc( xx0, yy0, cc0, ss0, m_k/ff, LL, qx, qy );
        int_type res = 1;
        if ( s < 0 || s > LL ) {
          s = m_L;
          eval_ISO( s, offs, x, y );
          // costruisco piano
          real_type nx = x-xx0;
          real_type ny = y-yy0;
          real_type dx = 2*qx-(xx0+x);
          real_type dy = 2*qy-(yy0+y);
          if ( nx*dx + ny*dy <= 0 ) {
            s = 0;
            x = xx0;
            y = yy0;
          }
          res = -1;
        } else {
          eval_ISO( s, offs, x, y );
        }
        real_type nx, ny;
        nor_ISO( s, nx, ny );
        real_type dx = qx-x;
        real_type dy = qy-y;
        t   = dx * nx + dy * ny + offs;
        dst = hypot( dx, dy );
        return res;
      }

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

      void
      CircleArc::paramNURBS(
        int_type & n_knots,
        int_type & n_pnts
      ) const {
        real_type dtheta = m_L*m_k;
        int_type  ns     = int_type(floor(3*abs(dtheta)/Utils::m_pi));
        if ( ns < 1 ) ns = 1;
        n_pnts  = 1+2*ns;
        n_knots = n_pnts+3;
      }

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

      void
      CircleArc::toNURBS(
        real_type * knots,
        real_type   Poly[][3]
      ) const {

        real_type dtheta = m_L*m_k;
        int_type  ns     = int_type(floor(3*abs(dtheta)/Utils::m_pi));
        if ( ns < 1 ) ns = 1;

        real_type th = dtheta/(2*ns);
        real_type w  = cos(th);
        real_type tg = tan(th)/2;

        real_type p0[2], p2[2];
        p0[0] = m_x0;
        p0[1] = m_y0;

        knots[0] = knots[1] = knots[2] = 0;
        Poly[0][0] = p0[0];
        Poly[0][1] = p0[1];
        Poly[0][2] = 1;

        real_type s  = 0;
        real_type ds = m_L/ns;
        int_type  kk = 0;
        for ( int_type i = 0; i < ns; ++i ) {
          s += ds;
          eval( s, p2[0], p2[1] );

          real_type nx = p0[1]-p2[1];
          real_type ny = p2[0]-p0[0];
          real_type xm = (p0[0]+p2[0])/2;
          real_type ym = (p0[1]+p2[1])/2;

          ++kk;
          Poly[kk][0] = w*(xm - nx * tg);
          Poly[kk][1] = w*(ym - ny * tg);
          Poly[kk][2] = w;

          ++kk;
          Poly[kk][0] = p2[0];
          Poly[kk][1] = p2[1];
          Poly[kk][2] = 1;

          knots[kk+1] = i+1;
          knots[kk+2] = i+1;

          p0[0] = p2[0];
          p0[1] = p2[1];

        }
        knots[kk+3] = ns;
      }

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

      real_type
      CircleArc::lenTolerance( real_type tol ) const {
        real_type absk = abs(m_k);
        real_type tmp  = absk*tol;
        if ( tmp > 0 ) {
          real_type dtheta = 2*(Utils::m_pi-acos(tmp-1));
          return dtheta/absk;
        } else {
          return m_L;
        }
      }

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

      ostream_type &
      operator << ( ostream_type & stream, CircleArc const & c ) {
        fmt::print( stream,
          "x0     = {}\n"
          "y0     = {}\n"
          "theta0 = {}\n"
          "k      = {}\n"
          "L      = {}\n",
          c.m_x0, c.m_y0, c.m_theta0, c.m_k, c.m_L
        );
        return stream;
      }

    }

    // EOF: Circle.cc