Program Listing for File G2lib.hxx

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

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


namespace G2lib {

  extern real_type const m_1_sqrt_pi;

  extern real_type const machepsi;
  extern real_type const machepsi10;
  extern real_type const machepsi100;
  extern real_type const machepsi1000;
  extern real_type const sqrtMachepsi;
  extern bool            intersect_with_AABBtree;

  #ifdef G2LIB_COMPATIBILITY_MODE

  extern bool use_ISO;

  static
  inline
  void
  lib_use_ISO()
  { use_ISO = true; }

  static
  inline
  void
  lib_use_SAE()
  { use_ISO = false; }

  #endif

  static
  inline
  void
  noAABBtree()
  { intersect_with_AABBtree = false; }

  static
  inline
  void
  yesAABBtree()
  { intersect_with_AABBtree = true; }

  /*
   * sin(x)/x
   */
  real_type Sinc( real_type x );
  real_type Sinc_D( real_type x );
  real_type Sinc_DD( real_type x );
  real_type Sinc_DDD( real_type x );

  /*
   * (1-cos(x))/x
   */
  real_type Cosc( real_type x );
  real_type Cosc_D( real_type x );
  real_type Cosc_DD( real_type x );
  real_type Cosc_DDD( real_type x );

  /*
   * atan(x)/x
   */
  real_type Atanc( real_type x );
  real_type Atanc_D( real_type x );
  real_type Atanc_DD( real_type x );
  real_type Atanc_DDD( real_type x );

  void rangeSymm( real_type & ang );

  // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
  inline
  void
  minmax3(
    real_type   a,
    real_type   b,
    real_type   c,
    real_type & vmin,
    real_type & vmax
  ) {
    vmin = vmax = a;
    if ( b < vmin ) vmin = b;
    else            vmax = b;
    if      ( c < vmin ) vmin = c;
    else if ( c > vmax ) vmax = c;
  }

  // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
  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
  projectPointOnCircle(
    real_type x0,
    real_type y0,
    real_type theta0,
    real_type k,
    real_type qx,
    real_type qy
  );

  // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
  inline
  bool
  pointInsideCircle(
    real_type x0,
    real_type y0,
    real_type c0,
    real_type s0,
    real_type k,
    real_type qx,
    real_type qy
  ) {
    real_type cx  = x0 - s0/k;
    real_type cy  = y0 + c0/k;
    real_type dst = hypot( qx - cx, qy - cy );
    return dst*k <= 1;
  }

  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
  );

  int_type
  solveLinearQuadratic2(
    real_type   A,
    real_type   B,
    real_type   C,
    real_type * x,
    real_type * y
  );

  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
  );

  /*\
   |   ____        _           ____       ____
   |  / ___|  ___ | |_   _____|___ \__  _|___ \
   |  \___ \ / _ \| \ \ / / _ \ __) \ \/ / __) |
   |   ___) | (_) | |\ V /  __// __/ >  < / __/
   |  |____/ \___/|_| \_/ \___|_____/_/\_\_____|
  \*/
  class Solve2x2 {
    int_type  i[2], j[2];
    real_type LU[2][2];
    real_type epsi;
    bool      singular;

  public:

    Solve2x2() : epsi(1e-10) {}
    bool factorize( real_type A[2][2] );

    bool solve( real_type const b[2], real_type x[2] ) const;
  };

  int_type
  isCounterClockwise(
    real_type const * P1,
    real_type const * P2,
    real_type const * P3
  );

  int_type
  isPointInTriangle(
    real_type const * pt,
    real_type const * P1,
    real_type const * P2,
    real_type const * P3
  );

  // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
  /*\
   |  __  ____   __  _          _____ _          _
   |  \ \/ /\ \ / / | |_ ___   |_   _| |__   ___| |_ __ _
   |   \  /  \ 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
  );

}