G2lib.hxx

/*--------------------------------------------------------------------------*\
 |                                                                          |
 |  Copyright (C) 2017                                                      |
 |                                                                          |
 |         , __                 , __                                        |
 |        /|/  \               /|/  \                                       |
 |         | __/ _   ,_         | __/ _   ,_                                |
 |         |   \|/  /  |  |   | |   \|/  /  |  |   |                        |
 |         |(__/|__/   |_/ \_/|/|(__/|__/   |_/ \_/|/                       |
 |                           /|                   /|                        |
 |                           \|                   \|                        |
 |                                                                          |
 |      Enrico Bertolazzi                                                   |
 |      Dipartimento di Ingegneria Industriale                              |
 |      Universita` degli Studi di Trento                                   |
 |      email: enrico.bertolazzi@unitn.it                                   |
 |                                                                          |
\*--------------------------------------------------------------------------*/
 
 
namespace G2lib {
 
  #ifndef DOXYGEN_SHOULD_SKIP_THIS
 
  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;
 
  extern integer const  G2LIB_AABB_CUT;
  extern integer const  G2LIB_AABB_MIN_NODES;
 
  // for CLOTHOIDS_BACK_COMPATIBILITY
  extern bool use_ISO;
 
  #endif
 
  #ifdef CLOTHOIDS_BACK_COMPATIBILITY
 
  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;
  }
 
  // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
 
  integer
  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[]
  );
 
  // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
 
  integer
  solveLinearQuadratic2(
    real_type A,
    real_type B,
    real_type C,
    real_type x[],
    real_type y[]
  );
 
  // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
 
  integer
  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 {
    integer   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;
  };
 
  integer
  is_counter_clockwise(
    real_type const P1[],
    real_type const P2[],
    real_type const P3[]
  );
 
 
  integer
  is_point_in_triangle(
    real_type const point[],
    real_type const P1[],
    real_type const P2[],
    real_type const P3[]
  );
 
  // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
  /*\
   |  __  ____   __  _          _____ _          _
   |  \ \/ /\ \ / / | |_ ___   |_   _| |__   ___| |_ __ _
   |   \  /  \ V /  | __/ _ \    | | | '_ \ / _ \ __/ _` |
   |   /  \   | |   | || (_) |   | | | | | |  __/ || (_| |
   |  /_/\_\  |_|    \__\___/    |_| |_| |_|\___|\__\__,_|
  \*/
  void
  xy_to_guess_angle(
    integer         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[]
  );
 
}