# Program Listing for File G2lib.cc

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

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

    #include "Clothoids.hh"
    #include "PolynomialRoots.hh"

    #include <algorithm>

    #ifdef __clang__
    #pragma clang diagnostic ignored "-Wglobal-constructors"
    #endif

    namespace G2lib {

      using std::numeric_limits;
      using std::fpclassify;
      using std::lower_bound;
      using std::abs;
      using std::sqrt;
      using std::sin;
      using std::cos;
      using std::tan;
      using std::atan;
      using std::asin;
      using std::acos;

      real_type const m_1_sqrt_pi  = 0.564189583547756286948079451561; // 1/sqrt(pi)

      real_type const machepsi     = numeric_limits<real_type>::epsilon();
      real_type const machepsi10   = 10*machepsi;
      real_type const machepsi100  = 100*machepsi;
      real_type const machepsi1000 = 1000*machepsi;
      real_type const sqrtMachepsi = sqrt(machepsi);
      bool            intersect_with_AABBtree = true;

      #ifdef G2LIB_COMPATIBILITY_MODE
      bool use_ISO = true;
      #endif

      char const *CurveType_name[] = {
        "LINE",
        "POLYLINE",
        "CIRCLE",
        "BIARC",
        "BIARC_LIST",
        "CLOTHOID",
        "CLOTHOID_LIST"
      };

      void
      rangeSymm( real_type & ang ) {
        ang = fmod( ang, Utils::m_2pi );
        while ( ang < -Utils::m_pi ) ang += Utils::m_2pi;
        while ( ang >  Utils::m_pi ) ang -= Utils::m_2pi;
      }

      #ifndef DOXYGEN_SHOULD_SKIP_THIS
      static inline real_type power2( real_type a ) { return a*a; }
      static inline real_type power3( real_type a ) { return a*a*a; }
      #endif

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

      /*
      // sin(x)/x
      */
      real_type
      Sinc( real_type x ) {
        if ( abs(x) < 0.02 ) {
          real_type x2 = x*x;
          return 1-(x2/6)*(1-(x2/20)*(1-x2/42));
        } else {
          return sin(x)/x;
        }
      }

      real_type
      Sinc_D( real_type x ) {
        real_type x2 = x*x;
        if ( abs(x) < 0.04 ) return -(x/3)*(1-(x2/10)*(1-(x2/28)*(1-(x2/54))));
        else                 return (cos(x)-sin(x)/x)/x;
      }

      real_type
      Sinc_DD( real_type x ) {
        real_type x2 = x*x;
        if ( abs(x) < 0.02 ) return -1./3.+x2*(0.1-x2*((1.0/168.0)-(x2/6480)));
        else                 return ((2/x2-1)*sin(x)-2*cos(x)/x)/x;
      }

      real_type
      Sinc_DDD( real_type x ) {
        real_type x2 = x*x;
        if ( abs(x) < 0.009 ) return (1.0/5.0+(-1.0/42.0+(1.0/1080.0)*x2)*x2)*x;
        else                  return ((6/x2-1)*cos(x)+(3-6/x2)*sin(x)/x)/x;
      }

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

      /*
      // (1-cos(x))/x
      */
      real_type
      Cosc( real_type x ) {
        if ( abs(x) < 0.04 ) {
          real_type x2 = x*x;
          return (x/2)*(1-(x2/12)*(1-(x2/30)*(1-x2/56)));
        } else {
          return (1-cos(x))/x;
        }
      }

      real_type
      Cosc_D( real_type x ) {
        if ( abs(x) < 0.02 ) {
          real_type x2  = x*x;
          return 0.5*(1-(x2/4)*(1-(x2/18)*(1-(x2/40))));
        } else {
          return (sin(x)+(cos(x)-1)/x)/x;
        }
      }

      real_type
      Cosc_DD( real_type x ) {
        real_type x2  = x*x;
        if ( abs(x) < 0.04 )
          return -(x/4)*(1-(x2/9)*(1-((3.0/80.0)*x2)*(1-((2.0/105.0)*x2))));
        else
          return ((1-2/x2)*cos(x)+(2/x-sin(x))/x)/x;
      }

      real_type
      Cosc_DDD( real_type x ) {
        real_type x2 = x*x;
        if ( abs(x) < 0.02 )
          return -(1-(x2/3)*(1-(x2/16)*(1-(2.0/75.0)*x2)))/4.0;
        else
          return ((6/x2-1)*sin(x)+((6/x2-3)*cos(x)-6/x2)/x)/x;
      }

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

      /*
      // atan(x)/x
      */
      real_type
      Atanc( real_type x ) {
        if ( abs(x) < 0.03 ) {
          real_type x2 = x*x;
          return 1-x2*((1./3.)-x2*((1./5.)-x2*((1./7.)-x2*((1./9.)-(x2/11)))));
        } else {
          return atan(x)/x;
        }
      }

      real_type
      Atanc_D( real_type x ) {
        real_type x2 = x*x;
        if ( abs(x) < 0.03 ) {
          return x*( -(2./3.) + x2*( (4./5.) + x2*( -(6./7.) + x2*( (8./9.) + x2*( -(10./11.) + x2*(12./13.))))));
        } else {
          return (1/(1+x2)-atan(x)/x)/x;
        }
      }

      real_type
      Atanc_DD( real_type x ) {
        real_type x2 = x*x;
        if ( abs(x) < 0.02 ) {
          return -2./3.+ x2*( (12./5.) + (-(30./7.) + x2 * ( (56./9.) + x2*( -(90./11.) + x2 * (132./13.)))));
        } else {
          return (2*atan(x)/x-(4*x2+2)/power2(1+x2))/x2;
        }
      }

      real_type
      Atanc_DDD( real_type x ) {
        real_type x2 = x*x;
        if ( abs(x) < 0.02 ) {
          return x*(24./5.+x2*(-120./7. + x2 * (112./3. + x2 * (-720./11. + x2*(1320./13. - x2*728./5.)))));
        } else {
          return ( ((18*x2+16)*x2+6)/power3(x2+1)-6*atan(x)/x )/(x2*x);
        }
      }

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

      #ifndef DOXYGEN_SHOULD_SKIP_THIS
      static
      real_type
      maxabs3( real_type A, real_type B, real_type C ) {
        real_type res  = abs(A);
        real_type absB = abs(B);
        if ( res < absB ) res = absB;
        real_type absC = abs(C);
        if ( res < absC ) res = absC;
        return res;
      }
      #endif

      int_type
      solveLinearQuadratic(
        real_type   A,
        real_type   B,
        real_type   C,
        real_type   a,
        real_type   b,
        real_type   c,
        real_type * x,
        real_type * y
      ) {
        real_type m1 = maxabs3(A,B,C);
        real_type m2 = maxabs3(a,b,c);
        A /= m1; B /= m1; C /= m1;
        a /= m2; b /= m2; c /= m2;
        real_type Ab   = A * b;
        real_type Ba   = B * a;
        real_type tmp  = A * Ab + B * Ba;
        real_type disc = tmp - (C * C) * a * b;
        real_type ACb  = Ab*C;
        real_type BCa  = Ba*C;
        if ( disc > machepsi100 ) {
          // two solution
          disc = sqrt(disc);
          real_type Bdisc = B*disc;
          real_type Adisc = A*disc;
          x[0] = (ACb-Bdisc)/tmp;
          x[1] = (ACb+Bdisc)/tmp;
          y[0] = (BCa+Adisc)/tmp;
          y[1] = (BCa-Adisc)/tmp;
          return 2;
        } if ( disc > -machepsi100 ) {
          // one solution
          x[0] = ACb/tmp;
          y[0] = BCa/tmp;
          return 1;
        }
        return 0; // no solution
      }

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

      int_type
      solveLinearQuadratic2(
        real_type   A,
        real_type   B,
        real_type   C,
        real_type * x,
        real_type * y
      ) {
        real_type m = maxabs3(A,B,C);
        A /= m; B /= m; C /= m;
        real_type tmp  = A*A + B*B;
        real_type disc = tmp - (C * C);
        real_type AC   = A*C;
        real_type BC   = B*C;
        if ( disc > machepsi100 ) {
          // two solution
          disc = sqrt(disc);
          real_type Bdisc = B*disc;
          real_type Adisc = A*disc;
          x[0] = (AC-Bdisc)/tmp;
          x[1] = (AC+Bdisc)/tmp;
          y[0] = (BC+Adisc)/tmp;
          y[1] = (BC-Adisc)/tmp;
          return 2;
        } if ( disc > -machepsi100 ) {
          // one solution
          x[0] = AC/tmp;
          y[0] = BC/tmp;
          return 1;
        }
        return 0; // no solution
      }

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

      #ifndef DOXYGEN_SHOULD_SKIP_THIS

      static
      int_type
      solveNLsysCircleCircle(
        real_type kA,
        real_type T,
        real_type Tx,
        real_type Ty,
        real_type kB,
        real_type x[2],
        real_type y[2]
      ) {
        real_type Tx2 = Tx*Tx;
        real_type Ty2 = Ty*Ty;
        real_type kB2 = kB*kB;
        real_type kA2 = kA*kA;
        real_type a   = (Tx2+Ty2)*kB2/4+Tx*kA*kB+kA2;
        real_type b   = (Tx*kB+2*kA)*T-Ty2;
        real_type c   = T*T;
        PolynomialRoots::Quadratic qsolve( a, b, c);
        if ( qsolve.complexRoots() ) return 0;
        real_type z[2] = { qsolve.real_root0(), qsolve.real_root1() };
        int_type nint = 0;
        for ( int_type i = 0; i < qsolve.numRoots(); ++i ) {
          real_type tmp = z[i]*(4-kB2*z[i]);
          if ( tmp < 0 ) continue;
          real_type xx = kB*z[i]/2;
          real_type yy = sqrt( tmp )/2;
          tmp = Tx*xx+kA*z[i]+T;
          if ( abs(tmp-Ty*yy) < abs(tmp+Ty*yy) ) yy = -yy;
          x[nint] = xx;
          y[nint] = yy;
          ++nint;
        }
        return nint;
      }

      static
      real_type
      invCoscSinc( real_type k, real_type x, real_type y ) {
        real_type ds, s = y;
        if ( abs(k) > sqrtMachepsi ) s = atan2( y*k, 1-k*x )/k;
        int_type iter = 0;
        do {
          real_type sk = s*k;
          ds = (y-Sinc(sk)*s)*cos(sk)/(1-sin(sk)*k*y);
          s += ds;
        } while ( abs(ds) > machepsi100 && ++iter < 5 );
        return s;
      }

      #endif

      int_type
      intersectCircleCircle(
        real_type   x1,
        real_type   y1,
        real_type   theta1,
        real_type   kappa1,
        real_type   x2,
        real_type   y2,
        real_type   theta2,
        real_type   kappa2,
        real_type * s1,
        real_type * s2
      ) {
        real_type dx    = x2 - x1;
        real_type dy    = y2 - y1;
        real_type L2    = dx*dx+dy*dy;
        real_type L     = sqrt(L2);
        real_type alpha = atan2( dy, dx );
        real_type a1    = alpha-theta1;
        real_type a2    = alpha-theta2;
        real_type t12   = theta1-theta2;
        real_type Sa1   = L*sin(a1);
        real_type Ca1   = L*cos(a1);
        real_type Sa2   = L*sin(a2);
        real_type Ca2   = L*cos(a2);
        real_type S12   = sin(t12);
        real_type C12   = cos(t12);
        real_type T1    = L2*kappa2+2*Sa2;
        real_type T2    = L2*kappa1-2*Sa1;
        real_type xx1[2], yy1[2], xx2[2], yy2[3];
        int_type nsol;
        if ( abs(T1) > abs(T2) ) {
          real_type Tx1 = -2*(Sa1*kappa2+C12);
          real_type Ty1 = -2*(Ca1*kappa2+S12);
          nsol = solveNLsysCircleCircle( kappa2, T1, Tx1, Ty1, kappa1, xx1, yy1 );
          for ( int_type i = 0; i < nsol; ++i ) {
            xx2[i] = C12*xx1[i]+S12*yy1[i]-Sa2;
            yy2[i] = C12*yy1[i]-S12*xx1[i]-Ca2;
          }
        } else {
          real_type Tx2 = 2*(Sa2*kappa1-C12);
          real_type Ty2 = 2*(Ca2*kappa1+S12);
          nsol = solveNLsysCircleCircle( kappa1, T2, Tx2, Ty2, kappa2, xx2, yy2 );
          for ( int_type i = 0; i < nsol; ++i ) {
            xx1[i] = C12*xx2[i]-S12*yy2[i]+Sa1;
            yy1[i] = C12*yy2[i]+S12*xx2[i]+Ca1;
          }
        }
        real_type len1 = Utils::m_2pi/(machepsi+abs(kappa1));
        real_type len2 = Utils::m_2pi/(machepsi+abs(kappa1));
        for ( int_type i = 0; i < nsol; ++i ) {
          real_type ss1 = invCoscSinc( kappa1, xx1[i], yy1[i] );
          real_type ss2 = invCoscSinc( kappa2, xx2[i], yy2[i] );
          while ( ss1 < 0    ) ss1 += len1;
          while ( ss2 < 0    ) ss2 += len2;
          while ( ss1 > len1 ) ss1 -= len1;
          while ( ss2 > len2 ) ss2 -= len2;
          s1[i] = ss1;
          s2[i] = ss2;
        }
        return nsol;
      }

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

      void
      xy_to_guess_angle(
        int_type          npts,
        real_type const * x,
        real_type const * y,
        real_type       * theta,
        real_type       * theta_min,
        real_type       * theta_max,
        real_type       * omega,
        real_type       * len
      ) {
        //
        // Compute guess angles
        //
        int_type ne  = npts-1;
        int_type ne1 = npts-2;

        real_type dx = x[1]-x[0];
        real_type dy = y[1]-y[0];
        omega[0] = atan2(dy,dx);
        len[0]   = hypot(dy,dx);
        for ( int_type j = 1; j < ne; ++j ) {
          dx       = x[j+1]-x[j];
          dy       = y[j+1]-y[j];
          omega[j] = atan2(dy,dx);
          len[j]   = hypot(dy,dx);
          real_type domega = omega[j]-omega[j-1];
          domega  -= round(domega/Utils::m_2pi)*Utils::m_2pi;
          omega[j] = omega[j-1]+domega;
        }

        real_type const dangle = 0.99 * Utils::m_pi;

        theta[0]      = omega[0];
        theta_min[0]  = omega[0] - dangle;
        theta_max[0]  = omega[0] + dangle;

        theta[ne]     = omega[ne1];
        theta_min[ne] = omega[ne1] - dangle;
        theta_max[ne] = omega[ne1] + dangle;

        real_type omega_L = omega[0];
        real_type len_L   = len[0];
        for ( int_type j = 1; j < ne; ++j ) {
          real_type omega_R = omega[j];
          real_type len_R   = len[j];
          theta[j] = ( omega_L/len_L + omega_R / len_R ) / ( 1/len_L + 1/len_R );
          if ( omega_R > omega_L ) {
            theta_min[j] = omega_L;
            theta_max[j] = omega_R;
          } else {
            theta_min[j] = omega_R;
            theta_max[j] = omega_L;
          }
          theta_min[j] -= dangle;
          theta_max[j] += dangle;
          omega_L = omega_R;
          len_L   = len_R;
        }
      }

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

      bool
      Solve2x2::factorize( real_type A[2][2] ) {
        // full pivoting
        real_type Amax = abs(A[0][0]);
        real_type tmp  = abs(A[0][1]);
        int_type ij = 0;
        if ( tmp > Amax ) { ij = 1; Amax = tmp; }
        tmp = abs(A[1][0]);
        if ( tmp > Amax ) { ij = 2; Amax = tmp; }
        tmp = abs(A[1][1]);
        if ( tmp > Amax ) { ij = 3; Amax = tmp; }
        if ( Utils::isZero(Amax) ) return false;
        if ( (ij&0x01) == 0x01 ) { j[0] = 1; j[1] = 0; }
        else                     { j[0] = 0; j[1] = 1; }
        if ( (ij&0x02) == 0x02 ) { i[0] = 1; i[1] = 0; }
        else                     { i[0] = 0; i[1] = 1; }
        // apply factorization
        LU[0][0] = A[i[0]][j[0]];
        LU[0][1] = A[i[0]][j[1]];
        LU[1][0] = A[i[1]][j[0]];
        LU[1][1] = A[i[1]][j[1]];

        LU[1][0] /= LU[0][0];
        LU[1][1] -= LU[1][0]*LU[0][1];
        // check for singularity
        singular = abs( LU[1][1] ) < epsi;
        return true;
      }

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

      bool
      Solve2x2::solve( real_type const b[2], real_type x[2] ) const {
        if ( singular ) {
          // L^+ Pb
          real_type tmp = (b[i[0]] + LU[1][0]*b[i[1]]) /
                          ( (1+power2(LU[1][0]) ) * ( power2(LU[0][0])+power2(LU[0][1]) ) );
          x[j[0]] = tmp*LU[0][0];
          x[j[1]] = tmp*LU[0][1];
          // check consistency
          tmp = (LU[0][0]*x[j[0]]+LU[0][1]*x[j[1]]);
          return hypot( b[i[0]]-tmp, b[i[1]]+tmp*LU[1][0] ) < hypot(b[0],b[1])*epsi;
        } else { // non singular
          // L^(-1) Pb
          x[j[0]] = b[i[0]];
          x[j[1]] = b[i[1]]-LU[1][0]*x[j[0]];
          // U^(-1) x
          x[j[1]] /= LU[1][1];
          x[j[0]]  = (x[j[0]]-LU[0][1]*x[j[1]])/LU[0][0];
          return FP_INFINITE != fpclassify(x[0]) &&
                 FP_NAN      != fpclassify(x[0]) &&
                 FP_INFINITE != fpclassify(x[1]) &&
                 FP_NAN      != fpclassify(x[1]);
        }
      }

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

      int_type
      isCounterClockwise(
        real_type const * P1,
        real_type const * P2,
        real_type const * P3
      ) {
        real_type dx1 = P2[0] - P1[0];
        real_type dy1 = P2[1] - P1[1];
        real_type dx2 = P3[0] - P1[0];
        real_type dy2 = P3[1] - P1[1];
        real_type tol = machepsi10*(hypot(dx1,dy1)*hypot(dx2,dy2));
        real_type det = dx1*dy2 - dy1*dx2;
        if      ( det >  tol ) return  1;
        else if ( det < -tol ) return -1;
        return 0;
      }

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

      real_type
      projectPointOnCircleArc(
        real_type x0,
        real_type y0,
        real_type c0,
        real_type s0,
        real_type k,
        real_type L,
        real_type qx,
        real_type qy
      ) {
        real_type dx  = x0 - qx;
        real_type dy  = y0 - qy;
        real_type a0  = c0 * dy - s0 * dx;
        real_type b0  = s0 * dy + c0 * dx;
        real_type tmp = a0*k;

        if ( 1+2*tmp > 0 ) {

          tmp = b0/(1+tmp);
          tmp *= -Atanc(tmp*k); // lunghezza

          if ( tmp < 0 ) {
            real_type absk = abs(k);
            // if 2*pi*R + tmp <= L add 2*pi*R  to the solution
            if ( Utils::m_2pi <= absk*(L-tmp) ) tmp += Utils::m_2pi / absk;
          }

          return tmp;

        } else {

          real_type om = atan2( b0, a0+1/k );
          if ( k < 0 ) om += Utils::m_pi;
          real_type ss = -om/k;
          real_type t  = Utils::m_2pi/abs(k);
          if      ( ss < 0 ) ss += t;
          else if ( ss > t ) ss -= t;
          return ss;
        }
      }

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

      real_type
      projectPointOnCircle(
        real_type x0,
        real_type y0,
        real_type theta0,
        real_type k,
        real_type qx,
        real_type qy
      ) {
        real_type dx  = x0 - qx;
        real_type dy  = y0 - qy;
        real_type c0  = cos(theta0);
        real_type s0  = sin(theta0);
        real_type a0  = c0 * dy - s0 * dx;
        real_type b0  = s0 * dy + c0 * dx;
        real_type tmp = a0*k;

        if ( 1+2*tmp > 0 ) {
          tmp = b0/(1+tmp);
          tmp *= -Atanc(tmp*k); // lunghezza
          return tmp;
        } else {
          real_type om = atan2( b0, a0+1/k );
          if ( k < 0 ) {
            if ( om < 0 ) om += Utils::m_pi;
            else          om -= Utils::m_pi;
          }
          return -om/k;
        }
      }

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

      int_type
      isPointInTriangle(
        real_type const * point,
        real_type const * p1,
        real_type const * p2,
        real_type const * p3
      ) {
        int_type d = isCounterClockwise(p1, p2, p3);
        int_type a = isCounterClockwise(p1, p2, point);
        int_type b = isCounterClockwise(p2, p3, point);
        int_type c = isCounterClockwise(p3, p1, point);
        if ( d < 0) { a = -a; b = -b; c = -c; }
        if ( a < 0 ) return -1;
        if ( b < 0 ) return -1;
        if ( c < 0 ) return -1;
        if ( a+b+c == 3 ) return 1;
        return 0;
      }

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

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

      real_type
      BaseCurve::tx( real_type s ) const
      { return cos(theta(s)); }

      real_type
      BaseCurve::tx_D( real_type s ) const
      { return -sin(theta(s))*theta_D(s); }

      real_type
      BaseCurve::tx_DD( real_type s ) const {
        real_type th    = theta(s);
        real_type th_D  = theta_D(s);
        real_type th_DD = theta_DD(s);
        real_type C     = cos(th);
        real_type S     = sin(th);
        return -(th_DD*S+(th_D*th_D)*C);
      }

      real_type
      BaseCurve::tx_DDD( real_type s ) const {
        real_type th     = theta(s);
        real_type th_D   = theta_D(s);
        real_type th_DD  = theta_DD(s);
        real_type th_DDD = theta_DDD(s);
        real_type C      = cos(th);
        real_type S      = sin(th);
        return (th_D*th_D*th_D-th_DDD)*S-3*th_DD*th_D*C;
      }

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

      real_type
      BaseCurve::ty( real_type s ) const
      { return sin(theta(s)); }

      real_type
      BaseCurve::ty_D( real_type s ) const
      { return cos(theta(s))*theta_D(s); }

      real_type
      BaseCurve::ty_DD( real_type s ) const {
        real_type th    = theta(s);
        real_type th_D  = theta_D(s);
        real_type th_DD = theta_DD(s);
        real_type C     = cos(th);
        real_type S     = sin(th);
        return th_DD*C-(th_D*th_D)*S;
      }

      real_type
      BaseCurve::ty_DDD( real_type s ) const {
        real_type th     = theta(s);
        real_type th_D   = theta_D(s);
        real_type th_DD  = theta_DD(s);
        real_type th_DDD = theta_DDD(s);
        real_type C      = cos(th);
        real_type S      = sin(th);
        return (th_DDD-th_D*th_D*th_D)*C-3*(th_DD*th_D)*S;
      }

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

      real_type
      BaseCurve::X_ISO( real_type s, real_type offs ) const
      { return X(s) + offs * nx_ISO(s); }

      real_type
      BaseCurve::Y_ISO( real_type s, real_type offs ) const
      { return Y(s) + offs * ny_ISO(s); }

      real_type
      BaseCurve::X_ISO_D( real_type s, real_type offs ) const
      { return X_D(s) + offs * nx_ISO_D(s); }

      real_type
      BaseCurve::Y_ISO_D( real_type s, real_type offs ) const
      { return Y_D(s) + offs * ny_ISO_D(s); }

      real_type
      BaseCurve::X_ISO_DD( real_type s, real_type offs ) const
      { return X_DD(s) + offs * nx_ISO_DD(s); }

      real_type
      BaseCurve::Y_ISO_DD( real_type s, real_type offs ) const
      { return Y_DD(s) + offs * ny_ISO_DD(s); }

      real_type
      BaseCurve::X_ISO_DDD( real_type s, real_type offs ) const
      { return X_DDD(s) + offs * nx_ISO_DDD(s); }

      real_type
      BaseCurve::Y_ISO_DDD( real_type s, real_type offs ) const
      { return Y_DDD(s) + offs * ny_ISO_DDD(s); }

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

      void
      BaseCurve::eval_ISO(
        real_type   s,
        real_type   offs,
        real_type & x,
        real_type & y
      ) const {
        real_type nx, ny;
        nor_ISO( s, nx, ny );
        eval( s, x, y );
        x += offs * nx;
        y += offs * ny;
      }

      void
      BaseCurve::eval_ISO_D(
        real_type   s,
        real_type   offs,
        real_type & x_D,
        real_type & y_D
      ) const {
        real_type nx_D, ny_D;
        nor_ISO_D( s, nx_D, ny_D );
        eval_D( s, x_D, y_D );
        x_D += offs * nx_D;
        y_D += offs * ny_D;
      }

      void
      BaseCurve::eval_ISO_DD(
        real_type   s,
        real_type   offs,
        real_type & x_DD,
        real_type & y_DD
      ) const {
        real_type nx_DD, ny_DD;
        nor_ISO_D( s, nx_DD, ny_DD );
        eval_DD( s, x_DD, y_DD );
        x_DD += offs * nx_DD;
        y_DD += offs * ny_DD;
      }

      void
      BaseCurve::eval_ISO_DDD(
        real_type   s,
        real_type   offs,
        real_type & x_DDD,
        real_type & y_DDD
      ) const {
        real_type nx_DDD, ny_DDD;
        nor_ISO_D( s, nx_DDD, ny_DDD );
        eval_DDD( s, x_DDD, y_DDD );
        x_DDD += offs * nx_DDD;
        y_DDD += offs * ny_DDD;
      }

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

    }

    // EOF: G2lib.cc