# Program Listing for File ClothoidG2.cc

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

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

    #include "Clothoids.hh"

    #include <cfloat>

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

    namespace G2lib {

      #ifndef DOXYGEN_SHOULD_SKIP_THIS

      using std::abs;
      using std::fpclassify;
      using std::copy;
      using std::back_inserter;
      using std::fill;
      using std::vector;

      inline
      real_type
      power2( real_type a )
      { return a*a; }

      inline
      real_type
      power3( real_type a )
      { return a*a*a; }

      inline
      real_type
      power4( real_type a )
      { real_type a2 = a*a; return a2*a2; }

      #endif

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

      int
      G2solve2arc::build(
        real_type _x0,
        real_type _y0,
        real_type _theta0,
        real_type _kappa0,
        real_type _x1,
        real_type _y1,
        real_type _theta1,
        real_type _kappa1
      ) {

        x0     = _x0;
        y0     = _y0;
        theta0 = _theta0;
        kappa0 = _kappa0;
        x1     = _x1;
        y1     = _y1;
        theta1 = _theta1;
        kappa1 = _kappa1;

        // scale problem
        real_type dx = x1 - x0;
        real_type dy = y1 - y0;
        phi    = atan2( dy, dx );
        lambda = hypot( dx, dy );

        real_type C = dx/lambda;
        real_type S = dy/lambda;
        lambda /= 2;

        xbar = -(x0*C+y0*S+lambda);
        ybar = x0*S-y0*C;

        th0 = theta0 - phi;
        th1 = theta1 - phi;

        k0 = kappa0*lambda;
        k1 = kappa1*lambda;

        DeltaK     = k1  - k0;
        DeltaTheta = th1 - th0;

        return solve();
      }

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

      void
      G2solve2arc::setTolerance( real_type tol ) {
        UTILS_ASSERT(
          tol > 0 && tol <= 0.1,
          "G2solve2arc::setTolerance, tolerance = {} must be in (0,0.1]\n", tol
        );
        tolerance = tol;
      }

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

      void
      G2solve2arc::setMaxIter( int miter ) {
        UTILS_ASSERT(
          miter > 0 && miter <= 1000,
          "G2solve2arc::setMaxIter, maxIter = {} must be in [1,1000]\n", miter
        );
        maxIter = miter;
      }

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

      void
      G2solve2arc::evalA(
        real_type   alpha,
        real_type   L,
        real_type & A
      ) const {
        real_type K  = k0+k1;
        real_type aK = alpha*DeltaK;
        A = alpha*(L*(aK-K)+2*DeltaTheta);
      }

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

      void
      G2solve2arc::evalA(
        real_type   alpha,
        real_type   L,
        real_type & A,
        real_type & A_1,
        real_type & A_2
      ) const {
        real_type K  = k0+k1;
        real_type aK = alpha*DeltaK;
        A   = alpha*(L*(aK-K)+2*DeltaTheta);
        A_1 = (2*aK-K)*L+2*DeltaTheta;
        A_2 = alpha*(aK-K);
      }

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

      void
      G2solve2arc::evalG(
        real_type alpha,
        real_type L,
        real_type th,
        real_type k,
        real_type G[2]
      ) const {
        real_type A, X, Y;
        evalA( alpha, L, A );
        real_type ak = alpha*k;
        GeneralizedFresnelCS( A, ak*L, th, X, Y );
        G[0] = alpha*X;
        G[1] = alpha*Y;
      }

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

      void
      G2solve2arc::evalG(
        real_type alpha,
        real_type L,
        real_type th,
        real_type k,
        real_type G[2],
        real_type G_1[2],
        real_type G_2[2]
      ) const {

        real_type A, A_1, A_2, X[3], Y[3];
        evalA( alpha, L, A, A_1, A_2 );
        real_type ak = alpha*k;
        real_type Lk = L*k;
        GeneralizedFresnelCS( 3, A, ak*L, th, X, Y );

        G[0]   = alpha*X[0];
        G_1[0] = X[0]-alpha*(Y[2]*A_1/2+Y[1]*Lk);
        G_2[0] =     -alpha*(Y[2]*A_2/2+Y[1]*ak);

        G[1]   = alpha*Y[0];
        G_1[1] = Y[0]+alpha*(X[2]*A_1/2+X[1]*Lk);
        G_2[1] =      alpha*(X[2]*A_2/2+X[1]*ak);

      }

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

      void
      G2solve2arc::evalF( real_type const vars[2], real_type F[2] ) const {
        real_type alpha = vars[0];
        real_type L     = vars[1];
        real_type G[2];
        evalG( alpha, L, th0, k0, G );
        F[0] = G[0] - 2/L;  F[1] = G[1];
        evalG( alpha-1, L, th1, k1, G );
        F[0] -= G[0]; F[1] -= G[1];
      }

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

      void
      G2solve2arc::evalFJ(
        real_type const vars[2],
        real_type       F[2],
        real_type       J[2][2]
      ) const {

        real_type alpha = vars[0];
        real_type L     = vars[1];
        real_type G[2], G_1[2], G_2[2];

        evalG( alpha, L, th0, k0, G, G_1, G_2 );

        F[0]    = G[0] - 2/L;       F[1]    = G[1];
        J[0][0] = G_1[0];           J[1][0] = G_1[1];
        J[0][1] = G_2[0] + 2/(L*L); J[1][1] = G_2[1];

        evalG( alpha-1, L, th1, k1, G, G_1, G_2 );
        F[0]    -= G[0];   F[1]    -= G[1];
        J[0][0] -= G_1[0]; J[1][0] -= G_1[1];
        J[0][1] -= G_2[0]; J[1][1] -= G_2[1];
      }

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

      int
      G2solve2arc::solve() {
        Solve2x2 solver;
        real_type X[2] = { 0.5, 2 };
        int iter = 0;
        bool converged = false;
        do {
          real_type F[2], J[2][2], d[2];
          evalFJ( X, F, J );
          if ( !solver.factorize( J ) ) break;
          solver.solve( F, d );
          real_type lenF = hypot(F[0],F[1]);
          #if 0
          X[0] -= d[0];
          X[1] -= d[1];
          #else
          real_type FF[2], dd[2], XX[2];
          // Affine invariant Newton solver
          real_type nd = hypot( d[0], d[1] );
          bool step_found = false;
          real_type tau = 2;
          do {
            tau  /= 2;
            XX[0] = X[0]-tau*d[0];
            XX[1] = X[1]-tau*d[1];
            evalF(XX, FF);
            solver.solve(FF, dd);
            step_found = hypot( dd[0], dd[1] ) <= (1-tau/2)*nd + 1e-6
                         && XX[0] > 0 && XX[0] < 1 && XX[1] > 0;
          } while ( tau > 1e-6 && !step_found );
          if ( !step_found ) break;
          X[0] = XX[0];
          X[1] = XX[1];
          #endif
          converged = lenF < tolerance;
        } while ( ++iter < maxIter && !converged );
        if ( converged ) converged = X[1] > 0 && X[0] > 0 && X[0] < 1;
        if ( converged ) buildSolution( X[0], X[1] );
        return converged ? iter : -1;
      }

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

      void
      G2solve2arc::buildSolution( real_type alpha, real_type L ) {
        real_type beta = 1-alpha;
        real_type s0   = L*alpha;
        real_type s1   = L*beta;
        real_type tmp  = 2*DeltaTheta-L*(k0+k1);
        real_type A0   = alpha*(s0*DeltaK+tmp);
        real_type A1   = beta*(s1*DeltaK-tmp);

        real_type dk0  = A0/(s0*s0);
        real_type dk1  = A1/(s1*s1);

        // transform solution from (-1,0)--(1,0) to (x0,y0)--(x1,y1)
        //S0.build( -1, 0, th0, k0, dk0, s0 );
        //S1.build(  1, 0, th1, k1, dk1, s1 );
        //S1.changeCurvilinearOrigin( -s1, s1 );
        s0  *= lambda;
        s1  *= lambda;
        dk0 /= lambda*lambda;
        dk1 /= lambda*lambda;

        S0.build( x0, y0, theta0, kappa0, dk0, s0 );
        S1.build( x1, y1, theta1, kappa1, dk1, s1 );
        S1.changeCurvilinearOrigin( -s1, s1 );
      }

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

      int
      G2solveCLC::build(
        real_type _x0,
        real_type _y0,
        real_type _theta0,
        real_type _kappa0,
        real_type _x1,
        real_type _y1,
        real_type _theta1,
        real_type _kappa1
      ) {

        x0     = _x0;
        y0     = _y0;
        theta0 = _theta0;
        kappa0 = _kappa0;
        x1     = _x1;
        y1     = _y1;
        theta1 = _theta1;
        kappa1 = _kappa1;

        // scale problem
        real_type dx = x1 - x0;
        real_type dy = y1 - y0;
        phi    = atan2( dy, dx );
        lambda = hypot( dx, dy );

        real_type C = dx/lambda;
        real_type S = dy/lambda;
        lambda /= 2;

        xbar = -(x0*C+y0*S+lambda);
        ybar = x0*S-y0*C;

        th0 = theta0 - phi;
        th1 = theta1 - phi;

        k0 = kappa0*lambda;
        k1 = kappa1*lambda;

        return solve();
      }

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

      void
      G2solveCLC::setTolerance( real_type tol ) {
        UTILS_ASSERT(
          tol > 0 && tol <= 0.1,
          "G2solveCLC::setTolerance, tolerance = {} must be in (0,0.1]\n", tol
        );
        tolerance = tol;
      }

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

      void
      G2solveCLC::setMaxIter( int miter ) {
        UTILS_ASSERT(
          miter > 0 && miter <= 1000,
          "G2solveCLC::setMaxIter, maxIter = {} must be in [1,1000]\n", miter
        );
        maxIter = miter;
      }

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

      int
      G2solveCLC::solve() {
        real_type X0[3], Y0[3], X1[3], Y1[3];
        real_type thM = 0, sM = 0.0;
        int iter = 0;
        bool converged = false;
        do {
          real_type D0 = thM - th0;
          real_type D1 = thM - th1;

          GeneralizedFresnelCS( 3, 2*D0, -2*D0, D0, X0, Y0 );
          GeneralizedFresnelCS( 3, 2*D1, -2*D1, D1, X1, Y1 );

          real_type F  = D0*k1*Y0[0]-D1*k0*Y1[0] - k0*k1*sin(thM);
          real_type dF = D0*k1*(X0[2]-2*X0[1]+X0[0])
                         - D1*k0*(X1[2]-2*X1[1]+X1[0])
                         - k0*k1*cos(thM)
                         + k1*Y0[0]-k0*Y1[0];

          if ( abs(dF) < 1e-10 ) break;
          real_type d = F/dF;
          #if 0
          thM -= d;
          #else
          real_type FF, dd, thM1;
          // Affine invariant Newton solver
          bool step_found = false;
          real_type tau = 2;
          do {
            tau  /= 2;
            thM1 = thM-tau*d;
            D0 = thM1 - th0;
            D1 = thM1 - th1;
            GeneralizedFresnelCS( 1, 2*D0, -2*D0, D0, X0, Y0 );
            GeneralizedFresnelCS( 1, 2*D1, -2*D1, D1, X1, Y1 );
            FF = D0*k1*Y0[0]-D1*k0*Y1[0] - k0*k1*sin(thM1);
            dd = FF/dF;
            step_found = abs( dd ) <= (1-tau/2)*abs(d) + 1e-6;
          } while ( tau > 1e-6 && !step_found );
          if ( !step_found ) break;
          thM = thM1;
          #endif
          converged = abs(d) < tolerance;
        } while ( ++iter < maxIter && !converged );
        if ( converged ) {
          real_type D0 = thM - th0;
          real_type D1 = thM - th1;
          GeneralizedFresnelCS( 1, 2*D0, -2*D0, D0, X0, Y0 );
          GeneralizedFresnelCS( 1, 2*D1, -2*D1, D1, X1, Y1 );
          sM = cos(thM) + D1*X1[0]/k1 - D0*X0[0]/k0;
          converged = sM > 0 && sM < 1e100;
        }
        if ( converged ) converged = buildSolution( sM, thM );
        return converged ? iter : -1;
      }

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

      bool
      G2solveCLC::buildSolution( real_type sM, real_type thM ) {
        real_type dk0 = 0.5*power2(k0/lambda)/(th0-thM);
        real_type dk1 = 0.5*power2(k1/lambda)/(th1-thM);
        real_type L0  = 2*lambda*(thM-th0)/k0;
        real_type L1  = 2*lambda*(th1-thM)/k1;

        if ( ! ( L0 > 0 && L1 > 0 ) ) return false;

        S0.build( x0, y0, theta0, kappa0, dk0, L0 );
        S1.build( x1, y1, theta1, kappa1, dk1, L1 );
        S1.changeCurvilinearOrigin( -L1, L1 );
        SM.build( S0.xEnd(), S0.yEnd(), S0.thetaEnd(), 0, 0, 2*sM*lambda );

        return true;
      }

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

      void
      G2solve3arc::setTolerance( real_type tol ) {
        UTILS_ASSERT(
          tol > 0 && tol <= 0.1,
          "G2solve3arc::setTolerance, tolerance = {} must be in (0,0.1]\n", tol
        );
        tolerance = tol;
      }

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

      void
      G2solve3arc::setMaxIter( int miter ) {
        UTILS_ASSERT(
          miter > 0 && miter <= 1000,
          "G2solve3arc::setMaxIter, maxIter = {} must be in [1,1000]\n", miter
        );
        maxIter = miter;
      }

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

      int
      G2solve3arc::build(
        real_type _x0,
        real_type _y0,
        real_type _theta0,
        real_type _kappa0,
        real_type _x1,
        real_type _y1,
        real_type _theta1,
        real_type _kappa1,
        real_type Dmax,
        real_type dmax
      ) {
        try {
          // save data
          x0     = _x0;
          y0     = _y0;
          theta0 = _theta0;
          kappa0 = _kappa0;
          x1     = _x1;
          y1     = _y1;
          theta1 = _theta1;
          kappa1 = _kappa1;

          // transform to reference frame
          real_type dx = x1 - x0;
          real_type dy = y1 - y0;
          phi    = atan2( dy, dx );
          Lscale = 2/hypot( dx, dy );

          th0 = theta0 - phi;
          th1 = theta1 - phi;

          // put in range
          rangeSymm(th0);
          rangeSymm(th1);

          K0 = (kappa0/Lscale); // k0
          K1 = (kappa1/Lscale); // k1

          if ( Dmax <= 0 ) Dmax = Utils::m_pi;
          if ( dmax <= 0 ) dmax = Utils::m_pi/8;

          if ( Dmax > Utils::m_2pi  ) Dmax = Utils::m_2pi;
          if ( dmax > Utils::m_pi/4 ) dmax = Utils::m_pi/4;

          // compute guess G1
          ClothoidCurve SG;
          SG.build_G1( -1, 0, th0, 1, 0, th1 );

          real_type kA = SG.kappaBegin();
          real_type kB = SG.kappaEnd();
          real_type dk = abs(SG.dkappa());
          real_type L3 = SG.length()/3;

          real_type tmp = 0.5*abs(K0-kA)/dmax;
          s0 = L3;
          if ( tmp*s0 > 1 ) s0 = 1/tmp;
          tmp = (abs(K0+kA)+s0*dk)/(2*Dmax);
          if ( tmp*s0 > 1 ) s0 = 1/tmp;

          tmp = 0.5*abs(K1-kB)/dmax;
          s1 = L3;
          if ( tmp*s1 > 1 ) s1 = 1/tmp;
          tmp = (abs(K1+kB)+s1*dk)/(2*Dmax);
          if ( tmp*s1 > 1 ) s1 = 1/tmp;

          real_type dth   = abs(th0-th1) / Utils::m_2pi;
          real_type scale = power3(cos( power4(dth)*Utils::m_pi_2 ));
          s0 *= scale;
          s1 *= scale;

          real_type L   = (3*L3-s0-s1)/2;
          real_type thM = SG.theta(s0+L);
          th0 = SG.thetaBegin();
          th1 = SG.thetaEnd();

          // setup

          K0 *= s0;
          K1 *= s1;

          real_type t0 = 2*th0+K0;
          real_type t1 = 2*th1-K1;

          c0  = s0*s1;
          c1  = 2 * s0;
          c2  = 0.25*((K1-6*(K0+th0)-2*th1)*s0 - 3*K0*s1);
          c3  = -c0 * (K0 + th0);
          c4  = 2 * s1;
          c5  = 0.25*((6*(K1-th1)-K0-2*th0)*s1 + 3*K1*s0);
          c6  = c0 * (K1 - th1);
          c7  = -0.5*(s0 + s1);
          c8  = th0 + th1 + 0.5*(K0 - K1);
          c9  = 0.25*(t1*s0 + t0*s1);
          c10 = 0.5*(s1 - s0);
          c11 = 0.5*(th1 - th0) - 0.25*(K0 + K1);
          c12 = 0.25*(t1*s0 - t0*s1);
          c13 = 0.5*s0*s1;
          c14 = 0.75*(s0 + s1);
          return solve( L, thM );
        } catch (...) {
          return -1;
          // nothing to do
        }
      }

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

      int
      G2solve3arc::build_fixed_length(
        real_type _s0,
        real_type _x0,
        real_type _y0,
        real_type _theta0,
        real_type _kappa0,
        real_type _s1,
        real_type _x1,
        real_type _y1,
        real_type _theta1,
        real_type _kappa1
      ) {
        try {
          // save data
          x0     = _x0;
          y0     = _y0;
          theta0 = _theta0;
          kappa0 = _kappa0;
          x1     = _x1;
          y1     = _y1;
          theta1 = _theta1;
          kappa1 = _kappa1;

          // transform to reference frame
          real_type dx = x1 - x0;
          real_type dy = y1 - y0;
          phi    = atan2( dy, dx );
          Lscale = 2/hypot( dx, dy );

          th0 = theta0 - phi;
          th1 = theta1 - phi;

          // put in range
          rangeSymm(th0);
          rangeSymm(th1);

          K0 = (kappa0/Lscale); // k0
          K1 = (kappa1/Lscale); // k1

          // compute guess G1
          ClothoidCurve SG;
          SG.build_G1( -1, 0, th0, 1, 0, th1 );

          s0 = _s0 * Lscale;
          s1 = _s1 * Lscale;

          real_type L   = (SG.length()-s0-s1)/2;
          real_type thM = SG.theta(s0+L);
          th0 = SG.thetaBegin();
          th1 = SG.thetaEnd();

          // setup

          K0 *= s0;
          K1 *= s1;

          real_type t0 = 2*th0+K0;
          real_type t1 = 2*th1-K1;

          c0  = s0*s1;
          c1  = 2 * s0;
          c2  = 0.25*((K1-6*(K0+th0)-2*th1)*s0 - 3*K0*s1);
          c3  = -c0 * (K0 + th0);
          c4  = 2 * s1;
          c5  = 0.25*((6*(K1-th1)-K0-2*th0)*s1 + 3*K1*s0);
          c6  = c0 * (K1 - th1);
          c7  = -0.5*(s0 + s1);
          c8  = th0 + th1 + 0.5*(K0 - K1);
          c9  = 0.25*(t1*s0 + t0*s1);
          c10 = 0.5*(s1 - s0);
          c11 = 0.5*(th1 - th0) - 0.25*(K0 + K1);
          c12 = 0.25*(t1*s0 - t0*s1);
          c13 = 0.5*s0*s1;
          c14 = 0.75*(s0 + s1);

          return solve( L, thM );

        } catch (...) {

          return -1;
          // nothing to do
        }
      }

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

      void
      G2solve3arc::evalF( real_type const vars[2], real_type F[2] ) const {

        real_type sM  = vars[0];
        real_type thM = vars[1];

        real_type dsM = 1.0 / (c13+(c14+sM)*sM);
        real_type dK0 = dsM*(c0*thM + sM*(c1*thM - K0*sM + c2) + c3);
        real_type dK1 = dsM*(c0*thM + sM*(c4*thM + K1*sM + c5) + c6);
        real_type dKM = dsM*sM*( thM*(c7-2*sM) + c8*sM + c9);
        real_type KM  = dsM*sM*(c10*thM + c11*sM + c12);

        real_type X0, Y0, X1, Y1, XMp, YMp, XMm, YMm;
        GeneralizedFresnelCS( dK0,  K0, th0, X0,  Y0);
        GeneralizedFresnelCS( dK1, -K1, th1, X1,  Y1);
        GeneralizedFresnelCS( dKM,  KM, thM, XMp, YMp);
        GeneralizedFresnelCS( dKM, -KM, thM, XMm, YMm);

        // in the standard problem dx = 2, dy = 0
        F[0] = s0*X0 + s1*X1 + sM*(XMm + XMp) - 2;
        F[1] = s0*Y0 + s1*Y1 + sM*(YMm + YMp) - 0;
      }

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

      void
      G2solve3arc::evalFJ(
        real_type const vars[2],
        real_type       F[2],
        real_type       J[2][2]
      ) const {

        real_type sM  = vars[0];
        real_type thM = vars[1];

        real_type dsM   = 1.0 / (c13+(c14+sM)*sM);
        real_type dsMsM = dsM*sM;
        real_type dK0   = dsM*(c0*thM + sM*(c1*thM + c2 - sM*K0) + c3);
        real_type dK1   = dsM*(c0*thM + sM*(c4*thM + c5 + sM*K1) + c6);
        real_type dKM   = dsMsM*(thM*(c7-2*sM) + c8*sM + c9);
        real_type KM    = dsMsM*(c10*thM + c11*sM + c12);

        real_type X0[3],  Y0[3],
                  X1[3],  Y1[3],
                  XMp[3], YMp[3],
                  XMm[3], YMm[3];
        GeneralizedFresnelCS( 3, dK0,  K0, th0, X0,  Y0);
        GeneralizedFresnelCS( 3, dK1, -K1, th1, X1,  Y1);
        GeneralizedFresnelCS( 3, dKM,  KM, thM, XMp, YMp);
        GeneralizedFresnelCS( 3, dKM, -KM, thM, XMm, YMm);

        // in the standard problem dx = 2, dy = 0
        real_type t0 = XMp[0]+XMm[0];
        real_type t1 = YMp[0]+YMm[0];
        F[0] = s0*X0[0] + s1*X1[0] + sM*t0 - 2;
        F[1] = s0*Y0[0] + s1*Y1[0] + sM*t1 - 0;

        // calcolo J(F)
        real_type dsM2 = dsM*dsM;
        real_type g0   = -(2 * sM + c14)*dsM2;
        real_type g1   = (c13 - sM*sM)*dsM2;
        real_type g2   = sM*(sM*c14+2*c13)*dsM2;

        real_type dK0_sM  = (c0*thM+c3)*g0 + (c1*thM+c2)*g1 - K0*g2;
        real_type dK1_sM  = (c0*thM+c6)*g0 + (c4*thM+c5)*g1 + K1*g2;
        real_type dKM_sM  = (c7*thM+c9)*g1 + (c8-2*thM)*g2;
        real_type KM_sM   = (c10*thM+c12)*g1 + c11*g2;

        real_type dK0_thM = (c0+c1*sM)*dsM;
        real_type dK1_thM = (c0+c4*sM)*dsM;
        real_type dKM_thM = (c7-2*sM)*dsMsM;
        real_type KM_thM  = c10*dsMsM;

        // coeff fresnel per f_j per lo jacobiano
        real_type f0 = -0.5*s0*Y0[2];
        real_type f1 = -0.5*s1*Y1[2];
        real_type f2 = -0.5*sM*(YMm[2] + YMp[2]);
        real_type f3 = sM*(YMm[1] - YMp[1]);
        real_type f4 = 0.5*s0*X0[2];
        real_type f5 = 0.5*s1*X1[2];
        real_type f6 = 0.5*sM*(XMm[2] + XMp[2]);
        real_type f7 = sM*(XMp[1] - XMm[1]);

        J[0][0] = f0 * dK0_sM  + f1 * dK1_sM  + f2 * dKM_sM  + f3 * KM_sM  + t0;
        J[0][1] = f0 * dK0_thM + f1 * dK1_thM + f2 * dKM_thM + f3 * KM_thM - sM * t1;
        J[1][0] = f4 * dK0_sM  + f5 * dK1_sM  + f6 * dKM_sM  + f7 * KM_sM  + t1;
        J[1][1] = f4 * dK0_thM + f5 * dK1_thM + f6 * dKM_thM + f7 * KM_thM + sM * t0;
      }

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

      int
      G2solve3arc::solve( real_type sM_guess, real_type thM_guess ) {

        Solve2x2 solver;
        real_type F[2], d[2], X[2], J[2][2];
        X[0] = sM_guess;
        X[1] = thM_guess;

        //real_type thmin = min(th0,th1)-2*m_2pi;
        //real_type thmax = max(th0,th1)+2*m_2pi;

        int iter = 0;
        bool converged = false;
        try {
          do {
            evalFJ(X, F, J);
            real_type lenF = hypot(F[0], F[1]);
            converged = lenF < tolerance;
            if ( converged || !solver.factorize(J) ) break;
            solver.solve(F, d);
            #if 1
            // use undamped Newton
            X[0] -= d[0];
            X[1] -= d[1];
            #else
            real_type FF[2], dd[2], XX[2];
            // Affine invariant Newton solver
            real_type nd = hypot( d[0], d[1] );
            bool step_found = false;
            real_type tau = 2;
            do {
              tau  /= 2;
              XX[0] = X[0]-tau*d[0];
              XX[1] = X[1]-tau*d[1];
              evalF(XX, FF);
              solver.solve(FF, dd);
              step_found = hypot( dd[0], dd[1] ) <= (1-tau/2)*nd + 1e-6;
                           //&& XX[0] > 0; // && XX[0] > X[0]/4 && XX[0] < 4*X[0];
                           //&& XX[1] > thmin && XX[1] < thmax;
            } while ( tau > 1e-6 && !step_found );
            if ( !step_found ) break;
            X[0] = XX[0];
            X[1] = XX[1];
            #endif
          } while ( ++iter < maxIter );

          // re-check solution
          if ( converged )
            converged = FP_INFINITE != fpclassify(X[0]) &&
                        FP_NAN      != fpclassify(X[0]) &&
                        FP_INFINITE != fpclassify(X[1]) &&
                        FP_NAN      != fpclassify(X[1]);
        }
        catch (...) {
          std::cerr << "G2solve3arc::solve, something go wrong\n";
          // nothing to do
        }
        if ( converged ) buildSolution(X[0], X[1]); // costruisco comunque soluzione
        return converged ? iter : -1;
      }

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

      void
      G2solve3arc::buildSolution( real_type sM, real_type thM ) {
        // soluzione nel frame di riferimento
        /* real_type k0 = K0
         S0.build( -1, 0, th0, k0, dK0,   0, L0 );
         S1.build( x1, y1, phi+th1, kappa1, dK1, -L1, 0  );
         S1.change_origin(-L1);
        */

        // ricostruzione dati clotoidi trasformati
        real_type dsM = 1.0 / (c13+(c14+sM)*sM);
        real_type dK0 = dsM*(c0*thM + sM*(c1*thM - K0*sM + c2) + c3);
        real_type dK1 = dsM*(c0*thM + sM*(c4*thM + K1*sM + c5) + c6);
        real_type dKM = dsM*sM*(c7*thM + sM*(c8 - 2*thM) + c9);
        real_type KM  = dsM*sM*(c10*thM + c11*sM + c12);

        real_type xa, ya, xmL, ymL;
        GeneralizedFresnelCS( dK0,  K0, th0, xa,  ya  );
        GeneralizedFresnelCS( dKM, -KM, thM, xmL, ymL );

        real_type xM = s0 * xa + sM * xmL - 1;
        real_type yM = s0 * ya + sM * ymL;

        // rovescia trasformazione standard
        real_type L0 = s0/Lscale;
        real_type L1 = s1/Lscale;
        real_type LM = sM/Lscale;

        dK0 *= power2(Lscale/s0);
        dK1 *= power2(Lscale/s1);
        dKM *= power2(Lscale/sM);
        KM  *= Lscale/sM;

        //th0 = theta0 - phi;
        //th1 = theta1 - phi;
        S0.build( x0, y0, phi+th0, kappa0, dK0, L0 );
        S1.build( x1, y1, phi+th1, kappa1, dK1, L1 );
        S1.changeCurvilinearOrigin( -L1, L1 );

        // la trasformazione inversa da [-1,1] a (x0,y0)-(x1,y1)
        // g(x,y) = RotInv(phi)*(1/lambda*[X;Y] - [xbar;ybar]) = [x;y]

        real_type C  = cos(phi);
        real_type S  = sin(phi);
        real_type dx = (xM + 1) / Lscale;
        real_type dy = yM / Lscale;
        SM.build(
          x0 + C * dx - S * dy,
          y0 + C * dy + S * dx,
          thM + phi, KM, dKM, 2*LM
        );
        SM.changeCurvilinearOrigin( -LM, 2*LM );
      }

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

      real_type
      G2solve3arc::thetaMinMax( real_type & thMin, real_type & thMax ) const {
        real_type thMin1, thMax1;
        S0.thetaMinMax( thMin,  thMax );
        S1.thetaMinMax( thMin1, thMax1 );
        if ( thMin > thMin1 ) thMin = thMin1;
        if ( thMax < thMax1 ) thMax = thMax1;
        SM.thetaMinMax( thMin1, thMax1 );
        if ( thMin > thMin1 ) thMin = thMin1;
        if ( thMax < thMax1 ) thMax = thMax1;
        return thMax-thMin;
      }

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

      real_type
      G2solve3arc::curvatureMinMax( real_type & kMin, real_type & kMax ) const {
        real_type kMin1, kMax1;
        S0.curvatureMinMax( kMin,  kMax );
        S1.curvatureMinMax( kMin1, kMax1 );
        if ( kMin > kMin1 ) kMin = kMin1;
        if ( kMax < kMax1 ) kMax = kMax1;
        SM.curvatureMinMax( kMin1, kMax1 );
        if ( kMin > kMin1 ) kMin = kMin1;
        if ( kMax < kMax1 ) kMax = kMax1;
        return kMax-kMin;
      }

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

      real_type
      G2solve3arc::theta( real_type s ) const {
        if ( s < S0.length() ) return S0.theta(s);
        s -= S0.length();
        if ( s < SM.length() ) return SM.theta(s);
        s -= S0.length();
        return S1.theta(s);
      }

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

      real_type
      G2solve3arc::theta_D( real_type s ) const {
        if ( s < S0.length() ) return S0.theta_D(s);
        s -= S0.length();
        if ( s < SM.length() ) return SM.theta_D(s);
        s -= S0.length();
        return S1.theta_D(s);
      }

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

      real_type
      G2solve3arc::theta_DD( real_type s ) const {
        if ( s < S0.length() ) return S0.theta_DD(s);
        s -= S0.length();
        if ( s < SM.length() ) return SM.theta_DD(s);
        s -= S0.length();
        return S1.theta_DD(s);
      }

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

      real_type
      G2solve3arc::theta_DDD( real_type s ) const {
        if ( s < S0.length() ) return S0.theta_DDD(s);
        s -= S0.length();
        if ( s < SM.length() ) return SM.theta_DDD(s);
        s -= S0.length();
        return S1.theta_DDD(s);
      }

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

      real_type
      G2solve3arc::X( real_type s ) const {
        if ( s < S0.length() ) return S0.X(s);
        s -= S0.length();
        if ( s < SM.length() ) return SM.X(s);
        s -= S0.length();
        return S1.X(s);
      }

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

      real_type
      G2solve3arc::Y( real_type s ) const {
        if ( s < S0.length() ) return S0.Y(s);
        s -= S0.length();
        if ( s < SM.length() ) return SM.Y(s);
        s -= S0.length();
        return S1.Y(s);
      }

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

      void
      G2solve3arc::eval(
        real_type   s,
        real_type & theta,
        real_type & kappa,
        real_type & x,
        real_type & y
      ) const {
        if ( s < S0.length() ) {
          S0.evaluate( s, theta, kappa, x, y );
        } else {
          s -= S0.length();
          if ( s < SM.length() ) {
            SM.evaluate( s, theta, kappa, x, y );
          } else {
            s -= SM.length();
            S1.evaluate( s, theta, kappa, x, y );
          }
        }
      }

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

      void
      G2solve3arc::eval(
        real_type   s,
        real_type & x,
        real_type & y
      ) const {
        if ( s < S0.length() ) {
          S0.eval(s, x, y );
        } else {
          s -= S0.length();
          if ( s < SM.length() ) {
            SM.eval(s, x, y );
          } else {
            s -= SM.length();
            S1.eval(s, x, y );
          }
        }
      }

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

      void
      G2solve3arc::eval_D(
        real_type   s,
        real_type & x_D,
        real_type & y_D
      ) const {
        if ( s < S0.length() ) {
          S0.eval_D(s, x_D, y_D );
        } else {
          s -= S0.length();
          if ( s < SM.length() ) {
            SM.eval_D(s, x_D, y_D );
          } else {
            s -= SM.length();
            S1.eval_D(s, x_D, y_D );
          }
        }
      }

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

      void
      G2solve3arc::eval_DD(
        real_type   s,
        real_type & x_DD,
        real_type & y_DD
      ) const {
        if ( s < S0.length() ) {
          S0.eval_DD(s, x_DD, y_DD );
        } else {
          s -= S0.length();
          if ( s < SM.length() ) {
            SM.eval_DD(s, x_DD, y_DD );
          } else {
            s -= SM.length();
            S1.eval_DD(s, x_DD, y_DD );
          }
        }
      }

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

      void
      G2solve3arc::eval_DDD(
        real_type   s,
        real_type & x_DDD,
        real_type & y_DDD
      ) const {
        if ( s < S0.length() ) {
          S0.eval_DDD(s, x_DDD, y_DDD );
        } else {
          s -= S0.length();
          if ( s < SM.length() ) {
            SM.eval_DDD(s, x_DDD, y_DDD );
          } else {
            s -= SM.length();
            S1.eval_DDD(s, x_DDD, y_DDD );
          }
        }
      }

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

      // offset curve
      void
      G2solve3arc::eval_ISO(
        real_type   s,
        real_type   offs,
        real_type & x,
        real_type & y
      ) const {
        if ( s < S0.length() ) {
          S0.eval_ISO( s, offs, x, y );
        } else {
          s -= S0.length();
          if ( s < SM.length() ) {
            SM.eval_ISO( s, offs, x, y );
          } else {
            s -= SM.length();
            S1.eval_ISO( s, offs, x, y );
          }
        }
      }

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

      void
      G2solve3arc::eval_ISO_D(
        real_type   s,
        real_type   offs,
        real_type & x_D,
        real_type & y_D
      ) const {
        if ( s < S0.length() ) {
          S0.eval_ISO_D( s, offs, x_D, y_D );
        } else {
          s -= S0.length();
          if ( s < SM.length() ) {
            SM.eval_ISO_D( s, offs, x_D, y_D );
          } else {
            s -= SM.length();
            S1.eval_ISO_D( s, offs, x_D, y_D );
          }
        }
      }

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

      void
      G2solve3arc::eval_ISO_DD(
        real_type   s,
        real_type   offs,
        real_type & x_DD,
        real_type & y_DD
      ) const {
        if ( s < S0.length() ) {
          S0.eval_ISO_DD( s, offs, x_DD, y_DD );
        } else {
          s -= S0.length();
          if ( s < SM.length() ) {
            SM.eval_ISO_DD( s, offs, x_DD, y_DD );
          } else {
            s -= SM.length();
            S1.eval_ISO_DD( s, offs, x_DD, y_DD );
          }
        }
      }

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

      void
      G2solve3arc::eval_ISO_DDD(
        real_type   s,
        real_type   offs,
        real_type & x_DDD,
        real_type & y_DDD
      ) const {
        if ( s < S0.length() ) {
          S0.eval_ISO_DDD( s, offs, x_DDD, y_DDD );
        } else {
          s -= S0.length();
          if ( s < SM.length() ) {
            SM.eval_ISO_DDD( s, offs, x_DDD, y_DDD );
          } else {
            s -= SM.length();
            S1.eval_ISO_DDD( s, offs, x_DDD, y_DDD );
          }
        }
      }

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

      void
      ClothoidSplineG2::guess(
        real_type * theta_guess,
        real_type * theta_min,
        real_type * theta_max
      ) const {
        size_t nn = size_t( m_npts );
        Utils::Malloc<real_type> mem( "ClothoidSplineG2::guess" );
        mem.allocate( 2*nn );
        real_type * omega = mem(nn);
        real_type * len   = mem(nn);
        G2lib::xy_to_guess_angle(
          m_npts, m_x, m_y, theta_guess, theta_min, theta_max, omega, len
        );
      }

      void
      ClothoidSplineG2::build(
        real_type const * xvec,
        real_type const * yvec,
        int_type          n
      ) {
        m_npts = n;
        size_t n1 = size_t(n-1);

        realValues.reallocate( 2*size_t(n) + 10 * n1 );

        m_x    = realValues( size_t(n) );
        m_y    = realValues( size_t(n) );
        m_k    = realValues( n1 );
        m_dk   = realValues( n1 );
        m_L    = realValues( n1 );
        m_kL   = realValues( n1 );
        m_L_1  = realValues( n1 );
        m_L_2  = realValues( n1 );
        m_k_1  = realValues( n1 );
        m_k_2  = realValues( n1 );
        m_dk_1 = realValues( n1 );
        m_dk_2 = realValues( n1 );
        std::copy_n( xvec, n, m_x );
        std::copy_n( yvec, n, m_y );
      }

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

      int_type
      ClothoidSplineG2::numTheta() const { return m_npts; }

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

      int_type
      ClothoidSplineG2::numConstraints() const {
        switch (m_tt) {
          case P1:
          case P2: return m_npts;
          default: break;
        }
        return m_npts-2;
      }

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

      bool
      ClothoidSplineG2::objective(
        real_type const * theta,
        real_type       & f
      ) const {
        ClothoidCurve cL, cR, c;
        int_type ne  = m_npts - 1;
        int_type ne1 = m_npts - 2;
        switch (m_tt) {
        case P1:
        case P2:
          f = 0;
          break;
        case P3:
          // forward target
          break;
        case P4:
          cL.build_G1( m_x[0],   m_y[0],   theta[0],   m_x[1],  m_y[1],  theta[1] );
          cR.build_G1( m_x[ne1], m_y[ne1], theta[ne1], m_x[ne], m_y[ne], theta[ne] );
          { real_type dk_L = cL.dkappa();
            real_type dk_R = cR.dkappa();
            f = dk_L*dk_L+dk_R*dk_R;
          }
          break;
        case P5:
          cL.build_G1( m_x[0],   m_y[0],   theta[0],   m_x[1],  m_y[1],  theta[1] );
          cR.build_G1( m_x[ne1], m_y[ne1], theta[ne1], m_x[ne], m_y[ne], theta[ne] );
          f = cL.length()+cR.length();
          break;
        case P6:
          f = 0;
          for ( int_type j = 0; j < ne; ++j ) {
            c.build_G1( m_x[j], m_y[j], theta[j], m_x[j+1], m_y[j+1], theta[j+1] );
            f += c.length();
          }
          break;
        case P7:
          f = 0;
          for ( int_type j = 0; j < ne; ++j ) {
            c.build_G1( m_x[j], m_y[j], theta[j], m_x[j+1], m_y[j+1], theta[j+1] );
            real_type Len  = c.length();
            real_type kur  = c.kappaBegin();
            real_type dkur = c.dkappa();
            f = f + Len * ( Len * ( dkur*( (dkur*Len)/3 + kur) ) + kur*kur );
          }
          break;
        case P8:
          f = 0;
          for ( int_type j = 0; j < ne; ++j ) {
            c.build_G1( m_x[j], m_y[j], theta[j], m_x[j+1], m_y[j+1], theta[j+1] );
            real_type Len  = c.length();
            real_type dkur = c.dkappa();
            f += Len*dkur*dkur;
          }
          break;
        case P9:
          f = 0;
          for ( int_type j = 0; j < ne; ++j ) {
            c.build_G1( m_x[j], m_y[j], theta[j], m_x[j+1], m_y[j+1], theta[j+1] );
            real_type Len  = c.length();
            real_type kur  = c.kappaBegin();
            real_type k2   = kur*kur;
            real_type k3   = k2*kur;
            real_type k4   = k2*k2;
            real_type dkur = c.dkappa();
            real_type dk2  = dkur*dkur;
            real_type dk3  = dkur*dk2;
            f = f + (k4+dk2+(2*k3*dkur+(2*k2*dk2+(dk3*(kur+dkur*Len/5))*Len)*Len)*Len)*Len;
          }
          break;
        }
        return true;
      }

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

      bool
      ClothoidSplineG2::gradient(
        real_type const * theta,
        real_type       * g
      ) const {
        ClothoidCurve cL, cR, c;
        real_type     LL_D[2], kL_D[2], dkL_D[2];
        real_type     LR_D[2], kR_D[2], dkR_D[2];
        std::fill_n( g, m_npts, 0 );
        int_type ne  = m_npts - 1;
        int_type ne1 = m_npts - 2;
        switch (m_tt) {
        case P1:
        case P2:
          break;
        case P3:
          break;
        case P4:
          cL.build_G1_D(
            m_x[0], m_y[0], theta[0],
            m_x[1], m_y[1], theta[1],
            LL_D, kL_D, dkL_D
          );
          cR.build_G1_D(
            m_x[ne1], m_y[ne1], theta[ne1],
            m_x[ne],  m_y[ne],  theta[ne],
            LR_D, kR_D, dkR_D
          );
          {
            real_type dkL = cL.dkappa();
            real_type dkR = cR.dkappa();
            g[0]   = 2*dkL*dkL_D[0];
            g[1]   = 2*dkL*dkL_D[1];
            g[ne1] = 2*dkR*dkR_D[0];
            g[ne]  = 2*dkR*dkR_D[1];
          }
          break;
        case P5:
          cL.build_G1_D(
            m_x[0], m_y[0], theta[0],
            m_x[1], m_y[1], theta[1],
            LL_D, kL_D, dkL_D
          );
          cR.build_G1_D(
            m_x[ne1], m_y[ne1], theta[ne1],
            m_x[ne],  m_y[ne],  theta[ne],
            LR_D, kR_D, dkR_D
          );
          g[0]   = LL_D[0];
          g[1]   = LL_D[1];
          g[ne1] = LR_D[0];
          g[ne]  = LR_D[1];
          break;
        case P6:
          for ( int_type j = 0; j < ne; ++j ) {
            real_type L_D[2], k_D[2], dk_D[2];
            c.build_G1_D(
              m_x[j],   m_y[j],   theta[j],
              m_x[j+1], m_y[j+1], theta[j+1],
              L_D, k_D, dk_D
            );
            g[j]   += L_D[0];
            g[j+1] += L_D[1];
          }
          break;
        case P7:
          for ( int_type j = 0; j < ne; ++j ) {
            real_type L_D[2], k_D[2], dk_D[2];
            c.build_G1_D(
              m_x[j],   m_y[j],   theta[j],
              m_x[j+1], m_y[j+1], theta[j+1],
              L_D, k_D, dk_D
            );
            real_type Len  = c.length();
            real_type L2   = Len*Len;
            real_type L3   = Len*L2;
            real_type kur  = c.kappaBegin();
            real_type k2   = kur*kur;
            real_type dkur = c.dkappa();
            real_type dk2  = dkur*dkur;
            g[j]   += 2*(dkur*dk_D[0]*L3)/3
                      + (dk2*L2*L_D[0])
                      + dk_D[0]*L2*kur
                      + 2*dkur*Len*L_D[0]*kur
                      + dkur*L2*k_D[0]
                      + L_D[0]*k2
                      + 2*Len*kur*k_D[0];
            g[j+1] += 2*(dkur*dk_D[1]*L3)/3
                      + (dk2*L2*L_D[1])
                      + dk_D[1]*L2*kur
                      + 2*dkur*Len*L_D[1]*kur
                      + dkur*L2*k_D[1]
                      + L_D[1]*k2
                      + 2*Len*kur*k_D[1];
          }
          break;
        case P8:
          for ( int_type j = 0; j < ne; ++j ) {
            real_type L_D[2], k_D[2], dk_D[2];
            c.build_G1_D(
              m_x[j],   m_y[j],   theta[j],
              m_x[j+1], m_y[j+1], theta[j+1],
              L_D, k_D, dk_D
            );
            real_type Len  = c.length();
            real_type dkur = c.dkappa();
            g[j]   += (2*Len*dk_D[0] + L_D[0]*dkur)*dkur;
            g[j+1] += (2*Len*dk_D[1] + L_D[1]*dkur)*dkur;
          }
          break;
        case P9:
          for ( int_type j = 0; j < ne; ++j ) {
            real_type L_D[2], k_D[2], dk_D[2];
            c.build_G1_D(
              m_x[j],   m_y[j],   theta[j],
              m_x[j+1], m_y[j+1], theta[j+1],
              L_D, k_D, dk_D
            );
            real_type Len  = c.length();
            real_type kur  = c.kappaBegin();
            real_type k2   = kur*kur;
            real_type k3   = kur*k2;
            real_type dkur = c.dkappa();
            real_type dk2  = dkur*dkur;
            real_type dkL  = dkur*Len;
            real_type A = ( ( (dkL+4*kur)*dkL + 6*k2)*dkL + 4*k3) * dkL + dk2 + k2*k2;
            real_type B = ( ( ( ( 3*kur + 0.8*dkL ) * dkL + 4*k2 ) * dkL +2*k3 ) * Len + 2*dkur ) * Len;
            real_type C = ( ( ( dkL + 4*kur ) * dkL + 6*k2 ) * dkL + 4*k3 ) * Len;
            g[j]   += A*L_D[0] + B*dk_D[0] + C*k_D[0];
            g[j+1] += A*L_D[1] + B*dk_D[1] + C*k_D[1];
          }
          break;
        }
        return true;
      }

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

      bool
      ClothoidSplineG2::constraints(
        real_type const * theta,
        real_type       * c
      ) const {
        ClothoidCurve cc;
        int_type ne  = m_npts - 1;
        int_type ne1 = m_npts - 2;

        for ( int_type j = 0; j < ne; ++j ) {
          cc.build_G1( m_x[j], m_y[j], theta[j], m_x[j+1], m_y[j+1], theta[j+1] );
          m_k[j]  = cc.kappaBegin();
          m_dk[j] = cc.dkappa();
          m_L[j]  = cc.length();
          m_kL[j] = m_k[j]+m_dk[j]*m_L[j];
        }

        for ( int_type j = 0; j < ne1; ++j ) c[j] = m_kL[j]-m_k[j+1];

        switch (m_tt) {
        case P1:
          c[ne1] = diff2pi( theta[0]  - m_theta_I );
          c[ne]  = diff2pi( theta[ne] - m_theta_F );
          break;
        case P2:
          c[ne1] = m_kL[ne1] - m_k[0];
          c[ne]  = diff2pi( theta[0] - theta[ne] );
          break;
        default:
          break;
        }
        return true;
      }

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

      int_type
      ClothoidSplineG2::jacobian_nnz() const {
        int_type nnz = 3*(m_npts-2);
        switch (m_tt) {
        case P1: nnz += 2; break;
        case P2: nnz += 6; break;
        default:            break;
        }
        return nnz;
      }

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

      bool
      ClothoidSplineG2::jacobian_pattern(
        int_type * ii,
        int_type * jj
      ) const {
        ClothoidCurve cc;
        int_type ne  = m_npts - 1;
        int_type ne1 = m_npts - 2;

        int_type kk = 0;
        for ( int_type j = 0; j < ne1; ++j ) {
          ii[kk] = j; jj[kk] = j; ++kk;
          ii[kk] = j; jj[kk] = j+1; ++kk;
          ii[kk] = j; jj[kk] = j+2; ++kk;
        }

        switch (m_tt) {
        case P1:
          ii[kk] = ne1; jj[kk] = 0; ++kk;
          ii[kk] = ne; jj[kk] = ne; ++kk;
          break;
        case P2:
          ii[kk] = ne1; jj[kk] = 0; ++kk;
          ii[kk] = ne1; jj[kk] = 1; ++kk;
          ii[kk] = ne1; jj[kk] = ne1; ++kk;
          ii[kk] = ne1; jj[kk] = ne; ++kk;
          ii[kk] = ne; jj[kk] = 0; ++kk;
          ii[kk] = ne; jj[kk] = ne; ++kk;
          break;
        default:
          break;
        }

        return true;
      }

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

      bool
      ClothoidSplineG2::jacobian_pattern_matlab(
        real_type * ii,
        real_type * jj
      ) const {
        ClothoidCurve cc;
        int_type ne  = m_npts - 1;
        int_type ne1 = m_npts - 2;

        int_type kk = 0;
        for ( int_type j = 1; j <= ne1; ++j ) {
          ii[kk] = j; jj[kk] = j;   ++kk;
          ii[kk] = j; jj[kk] = j+1; ++kk;
          ii[kk] = j; jj[kk] = j+2; ++kk;
        }

        switch (m_tt) {
        case P1:
          ii[kk] = ne;     jj[kk] = 1;      ++kk;
          ii[kk] = m_npts; jj[kk] = m_npts; ++kk;
          break;
        case P2:
          ii[kk] = ne;     jj[kk] = 1;      ++kk;
          ii[kk] = ne;     jj[kk] = 2;      ++kk;
          ii[kk] = ne;     jj[kk] = ne;     ++kk;
          ii[kk] = ne;     jj[kk] = m_npts; ++kk;
          ii[kk] = m_npts; jj[kk] = 1;      ++kk;
          ii[kk] = m_npts; jj[kk] = m_npts; ++kk;
          break;
        default:
          break;
        }

        return true;
      }

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

      bool
      ClothoidSplineG2::jacobian(
        real_type const * theta,
        real_type       * vals
      ) const {
        ClothoidCurve cc;
        int_type ne  = m_npts - 1;
        int_type ne1 = m_npts - 2;

        for ( int_type j = 0; j < ne; ++j ) {
          real_type L_D[2], k_D[2], dk_D[2];
          cc.build_G1_D(
            m_x[j],   m_y[j],   theta[j],
            m_x[j+1], m_y[j+1], theta[j+1],
            L_D, k_D, dk_D
          );
          m_k[j]    = cc.kappaBegin();
          m_dk[j]   = cc.dkappa();
          m_L[j]    = cc.length();
          m_kL[j]   = m_k[j]+m_dk[j]*m_L[j];
          m_L_1[j]  = L_D[0];  m_L_2[j]  = L_D[1];
          m_k_1[j]  = k_D[0];  m_k_2[j]  = k_D[1];
          m_dk_1[j] = dk_D[0]; m_dk_2[j] = dk_D[1];
        }

        int_type kk = 0;
        for ( int_type j = 0; j < ne1; ++j ) {
          vals[kk++] =  m_k_1[j] + m_dk_1[j]*m_L[j] + m_dk[j]*m_L_1[j];
          vals[kk++] =  m_k_2[j] + m_dk_2[j]*m_L[j] + m_dk[j]*m_L_2[j] - m_k_1[j+1];
          vals[kk++] = -m_k_2[j+1];
        }

        switch (m_tt) {
        case P1:
          vals[kk++] = 1;
          vals[kk++] = 1;
          break;
        case P2:
          vals[kk++] = -m_k_1[0];
          vals[kk++] = -m_k_2[0];
          vals[kk++] = m_k_1[ne1]+m_L_1[ne1]*m_dk[ne1]+m_L[ne1]*m_dk_1[ne1];
          vals[kk++] = m_k_2[ne1]+m_L_2[ne1]*m_dk[ne1]+m_L[ne1]*m_dk_2[ne1];
          vals[kk++] = 1;
          vals[kk++] = -1;
          break;
        default:
          break;
        }
        return true;
      }

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

      ostream_type &
      operator << ( ostream_type & stream, ClothoidSplineG2 const & c ) {
        fmt::print( stream,
          "npts   = {}\n"
          "target = {}\n",
          c.m_npts, int(c.m_tt)
        );
        return stream;
      }

    }

    // EOF: ClothoidG2.cc