# Program Listing for File Fresnel.hxx

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

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

    namespace G2lib {

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

      /*\
       |   _____                         _
       |  |  ___| __ ___  ___ _ __   ___| |
       |  | |_ | '__/ _ \/ __| '_ \ / _ \ |
       |  |  _|| | |  __/\__ \ | | |  __/ |
       |  |_|  |_|  \___||___/_| |_|\___|_|
      \*/
      /*
       * Compute Fresnel integrals
       */
      void
      FresnelCS(
        real_type   x,
        real_type & C,
        real_type & S
      );

      void
      FresnelCS(
        int_type    nk,
        real_type   x,
        real_type * C,
        real_type * S
      );

      void
      GeneralizedFresnelCS(
        int_type    nk,
        real_type   a,
        real_type   b,
        real_type   c,
        real_type * intC,
        real_type * intS
      );

      void
      GeneralizedFresnelCS(
        real_type   a,
        real_type   b,
        real_type   c,
        real_type & intC,
        real_type & intS
      );

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

      #ifndef DOXYGEN_SHOULD_SKIP_THIS

      class ClothoidData {
      public:

        real_type x0;
        real_type y0;
        real_type theta0;
        real_type kappa0;
        real_type dk;

        ClothoidData()
        : x0(0)
        , y0(0)
        , theta0(0)
        , kappa0(0)
        , dk(0)
        {}

        real_type
        deltaTheta( real_type s ) const
        { return s*(kappa0 + 0.5*s*dk); }

        real_type theta
        ( real_type s ) const
        { return theta0 + s*(kappa0 + 0.5*s*dk); }

        real_type theta_D  ( real_type s ) const { return kappa0 + s*dk; }
        real_type theta_DD ( real_type   ) const { return dk; }
        real_type theta_DDD( real_type   ) const { return 0; }

        real_type kappa    ( real_type s ) const { return kappa0 + s*dk; }
        real_type kappa_D  ( real_type   ) const { return dk; }
        real_type kappa_DD ( real_type   ) const { return 0; }
        real_type kappa_DDD( real_type   ) const { return 0; }

        real_type X( real_type s ) const;
        real_type Y( real_type s ) const;
        real_type X_D( real_type s ) const;
        real_type Y_D( real_type s ) const;
        real_type X_DD( real_type s ) const;
        real_type Y_DD( real_type s ) const;
        real_type X_DDD( real_type s ) const;
        real_type Y_DDD( real_type s ) const;

        // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
        real_type X_ISO    ( real_type s, real_type offs ) const;
        real_type Y_ISO    ( real_type s, real_type offs ) const;
        real_type X_ISO_D  ( real_type s, real_type offs ) const;
        real_type Y_ISO_D  ( real_type s, real_type offs ) const;
        real_type X_ISO_DD ( real_type s, real_type offs ) const;
        real_type Y_ISO_DD ( real_type s, real_type offs ) const;
        real_type X_ISO_DDD( real_type s, real_type offs ) const;
        real_type Y_ISO_DDD( real_type s, real_type offs ) const;

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

        real_type X_SAE    ( real_type s, real_type offs ) const;
        real_type Y_SAE    ( real_type s, real_type offs ) const;
        real_type X_SAE_D  ( real_type s, real_type offs ) const;
        real_type Y_SAE_D  ( real_type s, real_type offs ) const;
        real_type X_SAE_DD ( real_type s, real_type offs ) const;
        real_type Y_SAE_DD ( real_type s, real_type offs ) const;
        real_type X_SAE_DDD( real_type s, real_type offs ) const;
        real_type Y_SAE_DDD( real_type s, real_type offs ) const;

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

        real_type tg0_x() const { return cos(this->theta0); }
        real_type tg0_y() const { return sin(this->theta0); }

        real_type tg_x( real_type s ) const { return cos(this->theta(s)); }
        real_type tg_y( real_type s ) const { return sin(this->theta(s)); }

        real_type tg_x_D( real_type s ) const;
        real_type tg_y_D( real_type s ) const;

        real_type tg_x_DD( real_type s ) const;
        real_type tg_y_DD( real_type s ) const;

        real_type tg_x_DDD( real_type s ) const;
        real_type tg_y_DDD( real_type s ) const;

        real_type nor0_x_ISO() const { return -this->tg0_y(); }
        real_type nor0_y_ISO() const { return this->tg0_x(); }

        real_type nor_x_ISO( real_type s ) const { return -this->tg_y(s); }
        real_type nor_y_ISO( real_type s ) const { return this->tg_x(s); }

        real_type nor_x_ISO_D( real_type s ) const { return -this->tg_y_D(s); }
        real_type nor_y_ISO_D( real_type s ) const { return this->tg_x_D(s);  }

        real_type nor_x_ISO_DD( real_type s ) const { return -this->tg_y_DD(s);  }
        real_type nor_y_ISO_DD( real_type s ) const { return this->tg_x_DD(s); }

        real_type nor_x_ISO_DDD( real_type s ) const { return -this->tg_y_DDD(s); }
        real_type nor_y_ISO_DDD( real_type s ) const { return this->tg_x_DDD(s); }

        real_type nor0_x_SAE() const { return this->tg0_y(); }
        real_type nor0_y_SAE() const { return -this->tg0_x(); }

        real_type nor_x_SAE( real_type s ) const { return this->tg_y(s); }
        real_type nor_y_SAE( real_type s ) const { return -this->tg_x(s); }

        real_type nor_x_SAE_D( real_type s ) const { return this->tg_y_D(s); }
        real_type nor_y_SAE_D( real_type s ) const { return -this->tg_x_D(s);  }

        real_type nor_x_SAE_DD( real_type s ) const { return this->tg_y_DD(s);  }
        real_type nor_y_SAE_DD( real_type s ) const { return -this->tg_x_DD(s); }

        real_type nor_x_SAE_DDD( real_type s ) const { return this->tg_y_DDD(s); }
        real_type nor_y_SAE_DDD( real_type s ) const { return -this->tg_x_DDD(s); }

        void tg( real_type s, real_type & tx, real_type & ty ) const;
        void tg_D( real_type s, real_type & tx, real_type & ty ) const;
        void tg_DD( real_type s, real_type & tx, real_type & ty ) const;
        void tg_DDD( real_type s, real_type & tx, real_type & ty ) const;

        void nor_ISO( real_type s, real_type & nx, real_type & ny ) const;
        void nor_ISO_D( real_type s, real_type & nx_D, real_type & ny_D ) const;
        void nor_ISO_DD( real_type s, real_type & nx_DD, real_type & ny_DD ) const;
        void nor_ISO_DDD( real_type s, real_type & nx_DDD, real_type & ny_DDD ) const;

        void nor_SAE( real_type s, real_type & nx, real_type & ny ) const;
        void nor_SAE_D( real_type s, real_type & nx_D, real_type & ny_D ) const;
        void nor_SAE_DD( real_type s, real_type & nx_DD, real_type & ny_DD ) const;
        void nor_SAE_DDD( real_type s, real_type & nx_DDD, real_type & ny_DDD ) const;

        void
        evaluate(
          real_type   s,
          real_type & theta,
          real_type & kappa,
          real_type & x,
          real_type & y
        ) const;

        void
        eval(
          real_type   s,
          real_type & x,
          real_type & y
        ) const;

        void
        eval_D(
          real_type   s,
          real_type & x_D,
          real_type & y_D
        ) const;

        void
        eval_DD(
          real_type   s,
          real_type & x_DD,
          real_type & y_DD
        ) const;

        void
        eval_DDD(
          real_type   s,
          real_type & x_DDD,
          real_type & y_DDD
        ) const;

        void
        eval_ISO(
          real_type   s,
          real_type   offs,
          real_type & x,
          real_type & y
        ) const;

        void
        eval_ISO_D(
          real_type   s,
          real_type   offs,
          real_type & x_D,
          real_type & y_D
        ) const;

        void
        eval_ISO_DD(
          real_type   s,
          real_type   offs,
          real_type & x_DD,
          real_type & y_DD
        ) const;

        void
        eval_ISO_DDD(
          real_type   s,
          real_type   offs,
          real_type & x_DDD,
          real_type & y_DDD
        ) const;

        void
        eval_SAE(
          real_type   s,
          real_type   offs,
          real_type & x,
          real_type & y
        ) const {
          this->eval_ISO( s, -offs, x, y );
        }

        void
        eval_SAE_D(
          real_type   s,
          real_type   offs,
          real_type & x_D,
          real_type & y_D
        ) const {
          this->eval_ISO_D( s, -offs, x_D, y_D );
        }

        void
        eval_DAE_DD(
          real_type   s,
          real_type   offs,
          real_type & x_DD,
          real_type & y_DD
        ) const {
          this->eval_ISO_DD( s, -offs, x_DD, y_DD );
        }

        void
        eval_SAE_DDD(
          real_type   s,
          real_type   offs,
          real_type & x_DDD,
          real_type & y_DDD
        ) const {
          this->eval_ISO_DDD( s, -offs, x_DDD, y_DDD );
        }

        void
        eval( real_type s, ClothoidData & C ) const;

        real_type c0x() const { return x0 - (sin(theta0)/kappa0); }
        real_type c0y() const { return y0 + (cos(theta0)/kappa0); }

        void
        Pinfinity( real_type & x, real_type & y, bool plus ) const;

        void
        reverse( real_type L );

        void
        reverse( real_type L, ClothoidData & out) const;

        void
        rotate( real_type angle, real_type cx, real_type cy );

        void
        origin_at( real_type s_origin );

        real_type
        split_at_flex( ClothoidData & C0, ClothoidData & C1 ) const;

        real_type
        aplus( real_type dtheta ) const;

        bool
        bbTriangle(
          real_type   L,
          real_type & xx0, real_type & yy0,
          real_type & xx1, real_type & yy1,
          real_type & xx2, real_type & yy2
        ) const;

        bool
        bbTriangle_ISO(
          real_type   L,
          real_type   offs,
          real_type & xx0, real_type & yy0,
          real_type & xx1, real_type & yy1,
          real_type & xx2, real_type & yy2
        ) const;

        bool
        bbTriangle_SAE(
          real_type   L,
          real_type   offs,
          real_type & xx0, real_type & yy0,
          real_type & xx1, real_type & yy1,
          real_type & xx2, real_type & yy2
        ) const {
          return this->bbTriangle_ISO( L, -offs, xx0, yy0, xx1, yy1, xx2, yy2 );
        }

        int
        build_G1(
          real_type   x0,
          real_type   y0,
          real_type   theta0,
          real_type   x1,
          real_type   y1,
          real_type   theta1,
          real_type   tol,
          real_type & L,
          bool        compute_deriv = false,
          real_type   L_D[2]        = nullptr,
          real_type   k_D[2]        = nullptr,
          real_type   dk_D[2]       = nullptr
        );

        bool
        build_forward(
          real_type   x0,
          real_type   y0,
          real_type   theta0,
          real_type   kappa0,
          real_type   x1,
          real_type   y1,
          real_type   tol,
          real_type & L
        );

        void
        info( ostream_type & s ) const;

      };

      #endif

    }