Program Listing for File G2lib_intersect.cc

↰ Return to documentation for file (G2lib_intersect.cc)

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

#include "Clothoids.hh"

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

namespace G2lib {

  using std::map;
  using std::pair;

  using std::numeric_limits;
  using std::fpclassify;
  using std::lower_bound;
  using std::abs;
  using std::sqrt;

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

  #ifndef DOXYGEN_SHOULD_SKIP_THIS

  typedef pair<CurveType,CurveType> Ppair;

  // check if compiler is C++11
  #ifdef G2LIB_USE_CXX11
  static map<Ppair,CurveType> const promote_map = {
    {Ppair( G2LIB_LINE, G2LIB_LINE ),          G2LIB_LINE},
    {Ppair( G2LIB_LINE, G2LIB_CIRCLE ),        G2LIB_CIRCLE},
    {Ppair( G2LIB_LINE, G2LIB_CLOTHOID ),      G2LIB_CLOTHOID},
    {Ppair( G2LIB_LINE, G2LIB_BIARC ),         G2LIB_BIARC_LIST},
    {Ppair( G2LIB_LINE, G2LIB_BIARC_LIST ),    G2LIB_BIARC_LIST},
    {Ppair( G2LIB_LINE, G2LIB_CLOTHOID_LIST ), G2LIB_CLOTHOID_LIST},
    {Ppair( G2LIB_LINE, G2LIB_POLYLINE ),      G2LIB_POLYLINE},

    {Ppair( G2LIB_CIRCLE, G2LIB_LINE ),          G2LIB_CIRCLE},
    {Ppair( G2LIB_CIRCLE, G2LIB_CIRCLE ),        G2LIB_CIRCLE},
    {Ppair( G2LIB_CIRCLE, G2LIB_CLOTHOID ),      G2LIB_CLOTHOID},
    {Ppair( G2LIB_CIRCLE, G2LIB_BIARC ),         G2LIB_BIARC_LIST},
    {Ppair( G2LIB_CIRCLE, G2LIB_BIARC_LIST ),    G2LIB_BIARC_LIST},
    {Ppair( G2LIB_CIRCLE, G2LIB_CLOTHOID_LIST ), G2LIB_CLOTHOID_LIST},
    {Ppair( G2LIB_CIRCLE, G2LIB_POLYLINE ),      G2LIB_CLOTHOID_LIST},

    {Ppair( G2LIB_BIARC, G2LIB_LINE ),          G2LIB_CLOTHOID_LIST},
    {Ppair( G2LIB_BIARC, G2LIB_CIRCLE ),        G2LIB_CLOTHOID_LIST},
    {Ppair( G2LIB_BIARC, G2LIB_CLOTHOID ),      G2LIB_CLOTHOID_LIST},
    {Ppair( G2LIB_BIARC, G2LIB_BIARC ),         G2LIB_BIARC},
    {Ppair( G2LIB_BIARC, G2LIB_BIARC_LIST ),    G2LIB_BIARC_LIST},
    {Ppair( G2LIB_BIARC, G2LIB_CLOTHOID_LIST ), G2LIB_CLOTHOID_LIST},
    {Ppair( G2LIB_BIARC, G2LIB_POLYLINE ),      G2LIB_CLOTHOID_LIST},

    {Ppair( G2LIB_CLOTHOID, G2LIB_LINE ),          G2LIB_CLOTHOID},
    {Ppair( G2LIB_CLOTHOID, G2LIB_CIRCLE ),        G2LIB_CLOTHOID},
    {Ppair( G2LIB_CLOTHOID, G2LIB_CLOTHOID ),      G2LIB_CLOTHOID},
    {Ppair( G2LIB_CLOTHOID, G2LIB_BIARC ),         G2LIB_CLOTHOID_LIST},
    {Ppair( G2LIB_CLOTHOID, G2LIB_BIARC_LIST ),    G2LIB_CLOTHOID_LIST},
    {Ppair( G2LIB_CLOTHOID, G2LIB_CLOTHOID_LIST ), G2LIB_CLOTHOID_LIST},
    {Ppair( G2LIB_CLOTHOID, G2LIB_POLYLINE ),      G2LIB_CLOTHOID_LIST},

    {Ppair( G2LIB_CLOTHOID_LIST, G2LIB_LINE ),          G2LIB_CLOTHOID_LIST},
    {Ppair( G2LIB_CLOTHOID_LIST, G2LIB_CIRCLE ),        G2LIB_CLOTHOID_LIST},
    {Ppair( G2LIB_CLOTHOID_LIST, G2LIB_CLOTHOID ),      G2LIB_CLOTHOID_LIST},
    {Ppair( G2LIB_CLOTHOID_LIST, G2LIB_BIARC ),         G2LIB_CLOTHOID_LIST},
    {Ppair( G2LIB_CLOTHOID_LIST, G2LIB_BIARC_LIST ),    G2LIB_CLOTHOID_LIST},
    {Ppair( G2LIB_CLOTHOID_LIST, G2LIB_CLOTHOID_LIST ), G2LIB_CLOTHOID_LIST},
    {Ppair( G2LIB_CLOTHOID_LIST, G2LIB_POLYLINE ),      G2LIB_CLOTHOID_LIST},

    {Ppair( G2LIB_POLYLINE, G2LIB_LINE ),          G2LIB_POLYLINE},
    {Ppair( G2LIB_POLYLINE, G2LIB_CIRCLE ),        G2LIB_CLOTHOID_LIST},
    {Ppair( G2LIB_POLYLINE, G2LIB_CLOTHOID ),      G2LIB_CLOTHOID_LIST},
    {Ppair( G2LIB_POLYLINE, G2LIB_BIARC ),         G2LIB_CLOTHOID_LIST},
    {Ppair( G2LIB_POLYLINE, G2LIB_BIARC_LIST ),    G2LIB_CLOTHOID_LIST},
    {Ppair( G2LIB_POLYLINE, G2LIB_CLOTHOID_LIST ), G2LIB_CLOTHOID_LIST},
    {Ppair( G2LIB_POLYLINE, G2LIB_POLYLINE ),      G2LIB_POLYLINE}
  };
  #else
    static map<Ppair,CurveType> promote_map;
    static
    void
    init_promote_map() {
      static bool done = false;
      if ( done ) return;
      promote_map[ Ppair( G2LIB_LINE, G2LIB_LINE ) ]          = G2LIB_LINE;
      promote_map[ Ppair( G2LIB_LINE, G2LIB_CIRCLE ) ]        = G2LIB_CIRCLE;
      promote_map[ Ppair( G2LIB_LINE, G2LIB_CLOTHOID ) ]      = G2LIB_CLOTHOID;
      promote_map[ Ppair( G2LIB_LINE, G2LIB_BIARC ) ]         = G2LIB_BIARC_LIST;
      promote_map[ Ppair( G2LIB_LINE, G2LIB_BIARC_LIST ) ]    = G2LIB_BIARC_LIST;
      promote_map[ Ppair( G2LIB_LINE, G2LIB_CLOTHOID_LIST ) ] = G2LIB_CLOTHOID_LIST;
      promote_map[ Ppair( G2LIB_LINE, G2LIB_POLYLINE ) ]      = G2LIB_POLYLINE;

      promote_map[ Ppair( G2LIB_CIRCLE, G2LIB_LINE ) ]          = G2LIB_CIRCLE;
      promote_map[ Ppair( G2LIB_CIRCLE, G2LIB_CIRCLE ) ]        = G2LIB_CIRCLE;
      promote_map[ Ppair( G2LIB_CIRCLE, G2LIB_CLOTHOID ) ]      = G2LIB_CLOTHOID;
      promote_map[ Ppair( G2LIB_CIRCLE, G2LIB_BIARC ) ]         = G2LIB_BIARC_LIST;
      promote_map[ Ppair( G2LIB_CIRCLE, G2LIB_BIARC_LIST ) ]    = G2LIB_BIARC_LIST;
      promote_map[ Ppair( G2LIB_CIRCLE, G2LIB_CLOTHOID_LIST ) ] = G2LIB_CLOTHOID_LIST;
      promote_map[ Ppair( G2LIB_CIRCLE, G2LIB_POLYLINE ) ]      = G2LIB_CLOTHOID_LIST;

      promote_map[ Ppair( G2LIB_BIARC, G2LIB_LINE ) ]          = G2LIB_CLOTHOID_LIST;
      promote_map[ Ppair( G2LIB_BIARC, G2LIB_CIRCLE ) ]        = G2LIB_CLOTHOID_LIST;
      promote_map[ Ppair( G2LIB_BIARC, G2LIB_CLOTHOID ) ]      = G2LIB_CLOTHOID_LIST;
      promote_map[ Ppair( G2LIB_BIARC, G2LIB_BIARC ) ]         = G2LIB_BIARC;
      promote_map[ Ppair( G2LIB_BIARC, G2LIB_BIARC_LIST ) ]    = G2LIB_BIARC_LIST;
      promote_map[ Ppair( G2LIB_BIARC, G2LIB_CLOTHOID_LIST ) ] = G2LIB_CLOTHOID_LIST;
      promote_map[ Ppair( G2LIB_BIARC, G2LIB_POLYLINE ) ]      = G2LIB_CLOTHOID_LIST;

      promote_map[ Ppair( G2LIB_CLOTHOID, G2LIB_LINE ) ]          = G2LIB_CLOTHOID;
      promote_map[ Ppair( G2LIB_CLOTHOID, G2LIB_CIRCLE ) ]        = G2LIB_CLOTHOID;
      promote_map[ Ppair( G2LIB_CLOTHOID, G2LIB_CLOTHOID ) ]      = G2LIB_CLOTHOID;
      promote_map[ Ppair( G2LIB_CLOTHOID, G2LIB_BIARC ) ]         = G2LIB_CLOTHOID_LIST;
      promote_map[ Ppair( G2LIB_CLOTHOID, G2LIB_BIARC_LIST ) ]    = G2LIB_CLOTHOID_LIST;
      promote_map[ Ppair( G2LIB_CLOTHOID, G2LIB_CLOTHOID_LIST ) ] = G2LIB_CLOTHOID_LIST;
      promote_map[ Ppair( G2LIB_CLOTHOID, G2LIB_POLYLINE ) ]      = G2LIB_CLOTHOID_LIST;

      promote_map[ Ppair( G2LIB_CLOTHOID_LIST, G2LIB_LINE ) ]          = G2LIB_CLOTHOID_LIST;
      promote_map[ Ppair( G2LIB_CLOTHOID_LIST, G2LIB_CIRCLE ) ]        = G2LIB_CLOTHOID_LIST;
      promote_map[ Ppair( G2LIB_CLOTHOID_LIST, G2LIB_CLOTHOID ) ]      = G2LIB_CLOTHOID_LIST;
      promote_map[ Ppair( G2LIB_CLOTHOID_LIST, G2LIB_BIARC ) ]         = G2LIB_CLOTHOID_LIST;
      promote_map[ Ppair( G2LIB_CLOTHOID_LIST, G2LIB_BIARC_LIST ) ]    = G2LIB_CLOTHOID_LIST;
      promote_map[ Ppair( G2LIB_CLOTHOID_LIST, G2LIB_CLOTHOID_LIST ) ] = G2LIB_CLOTHOID_LIST;
      promote_map[ Ppair( G2LIB_CLOTHOID_LIST, G2LIB_POLYLINE ) ]      = G2LIB_CLOTHOID_LIST;

      promote_map[ Ppair( G2LIB_POLYLINE, G2LIB_LINE ) ]          = G2LIB_POLYLINE;
      promote_map[ Ppair( G2LIB_POLYLINE, G2LIB_CIRCLE ) ]        = G2LIB_CLOTHOID_LIST;
      promote_map[ Ppair( G2LIB_POLYLINE, G2LIB_CLOTHOID ) ]      = G2LIB_CLOTHOID_LIST;
      promote_map[ Ppair( G2LIB_POLYLINE, G2LIB_BIARC ) ]         = G2LIB_CLOTHOID_LIST;
      promote_map[ Ppair( G2LIB_POLYLINE, G2LIB_BIARC_LIST ) ]    = G2LIB_CLOTHOID_LIST;
      promote_map[ Ppair( G2LIB_POLYLINE, G2LIB_CLOTHOID_LIST ) ] = G2LIB_CLOTHOID_LIST;
      promote_map[ Ppair( G2LIB_POLYLINE, G2LIB_POLYLINE ) ]      = G2LIB_POLYLINE;
      done = true;
    }
  #endif

  #endif

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

  bool
  collision( BaseCurve const & obj1, BaseCurve const & obj2 ) {
    G2LIB_DEBUG_MESSAGE(
      "collision {} with {} using {}\n",
      CurveType_name[obj1.type()],
      CurveType_name[obj2.type()],
      CurveType_name[promote_map.at(Ppair(obj1.type(),obj2.type()))]
    );

    bool ok = false;
    switch ( promote_map.at(Ppair(obj1.type(),obj2.type())) ) {
    case G2LIB_LINE:
      {
        LineSegment L1( obj1 );
        LineSegment L2( obj2 );
        ok = L1.collision( L2 );
      }
      break;
    case G2LIB_CIRCLE:
      {
        CircleArc C1( obj1 );
        CircleArc C2( obj2 );
        ok = C1.collision( C2 );
      }
      break;
    case G2LIB_CLOTHOID:
      {
        ClothoidCurve C1( obj1 );
        ClothoidCurve C2( obj2 );
        ok = C1.collision( C2 );
      }
      break;
    case G2LIB_BIARC:
      {
        Biarc B1( obj1 );
        Biarc B2( obj2 );
        ok = B1.collision( B2 );
      }
      break;
    case G2LIB_BIARC_LIST:
      {
        BiarcList BL1( obj1 );
        BiarcList BL2( obj2 );
        ok = BL1.collision( BL2 );
      }
      break;
    case G2LIB_CLOTHOID_LIST:
      {
        ClothoidList CL1( obj1 );
        ClothoidList CL2( obj2 );
        ok = CL1.collision( CL2 );
      }
      break;
    case G2LIB_POLYLINE:
      {
        PolyLine PL1( obj1 );
        PolyLine PL2( obj2 );
        ok = PL1.collision( PL2 );
      }
      break;
    }
    return ok;
  }

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

  bool
  collision_ISO(
    BaseCurve const & obj1,
    real_type         offs1,
    BaseCurve const & obj2,
    real_type         offs2
  ) {
    #ifndef G2LIB_USE_CXX11
    init_promote_map();
    #endif

    G2LIB_DEBUG_MESSAGE(
      "collision (offs) {} with {} using {}\n",
      CurveType_name[obj1.type()],
      CurveType_name[obj2.type()],
      CurveType_name[promote_map.at(Ppair(obj1.type(),obj2.type()))]
    );

    bool ok = false;
    switch ( promote_map.at(Ppair(obj1.type(),obj2.type())) ) {
    case G2LIB_LINE:
      {
        LineSegment L1( obj1 );
        LineSegment L2( obj2 );
        ok = L1.collision_ISO( offs1, L2, offs2 );
      }
      break;
    case G2LIB_CIRCLE:
      {
        CircleArc C1( obj1 );
        CircleArc C2( obj2 );
        ok = C1.collision_ISO( offs1, C2, offs2 );
      }
      break;
    case G2LIB_CLOTHOID:
      {
        ClothoidCurve C1( obj1 );
        ClothoidCurve C2( obj2 );
        ok = C1.collision_ISO( offs1, C2, offs2 );
      }
      break;
    case G2LIB_BIARC:
      {
        Biarc B1( obj1 );
        Biarc B2( obj2 );
        ok = B1.collision_ISO( offs1, B2, offs2 );
      }
      break;
    case G2LIB_BIARC_LIST:
      {
        BiarcList BL1( obj1 );
        BiarcList BL2( obj2 );
        ok = BL1.collision_ISO( offs1, BL2, offs2 );
      }
      break;
    case G2LIB_CLOTHOID_LIST:
      {
        ClothoidList CL1( obj1 );
        ClothoidList CL2( obj2 );
        ok = CL1.collision_ISO( offs1, CL2, offs2 );
      }
      break;
    case G2LIB_POLYLINE:
      {
        PolyLine PL1( obj1 );
        PolyLine PL2( obj2 );
        ok = PL1.collision_ISO( offs1, PL2, offs2 );
      }
    }
    return ok;
  }

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

  void
  intersect(
    BaseCurve const & obj1,
    BaseCurve const & obj2,
    IntersectList   & ilist,
    bool              swap_s_vals
  ) {
    #ifndef G2LIB_USE_CXX11
    init_promote_map();
    #endif

    G2LIB_DEBUG_MESSAGE(
      "intersect {} with {} using {}\n",
      CurveType_name[obj1.type()],
      CurveType_name[obj2.type()],
      CurveType_name[promote_map.at(Ppair(obj1.type(),obj2.type()))]
    );

    switch ( promote_map.at(Ppair(obj1.type(),obj2.type())) ) {
    case G2LIB_LINE:
      {
        LineSegment L1( obj1 );
        LineSegment L2( obj2 );
        L1.intersect( L2, ilist, swap_s_vals );
      }
      break;
    case G2LIB_CIRCLE:
      {
        CircleArc C1( obj1 );
        CircleArc C2( obj2 );
        C1.intersect( C2, ilist, swap_s_vals );
      }
      break;
    case G2LIB_CLOTHOID:
      {
        ClothoidCurve C1( obj1 );
        ClothoidCurve C2( obj2 );
        C1.intersect( C2, ilist, swap_s_vals );
      }
      break;
    case G2LIB_BIARC:
      {
        Biarc B1( obj1 );
        Biarc B2( obj2 );
        B1.intersect( B2, ilist, swap_s_vals );
      }
      break;
    case G2LIB_BIARC_LIST:
      {
        BiarcList BL1( obj1 );
        BiarcList BL2( obj2 );
        BL1.intersect( BL2, ilist, swap_s_vals );
      }
      break;
    case G2LIB_CLOTHOID_LIST:
      {
        ClothoidList CL1( obj1 );
        ClothoidList CL2( obj2 );
        CL1.intersect( CL2, ilist, swap_s_vals );
      }
      break;
    case G2LIB_POLYLINE:
      {
        PolyLine PL1( obj1 );
        PolyLine PL2( obj2 );
        PL1.intersect( PL2, ilist, swap_s_vals );
      }
      break;
    }
  }

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

  void
  intersect_ISO(
    BaseCurve const & obj1,
    real_type         offs1,
    BaseCurve const & obj2,
    real_type         offs2,
    IntersectList   & ilist,
    bool              swap_s_vals
  ) {
    #ifndef G2LIB_USE_CXX11
    init_promote_map();
    #endif

    G2LIB_DEBUG_MESSAGE(
      "intersect (offs) {} with {} using {}\n",
      CurveType_name[obj1.type()],
      CurveType_name[obj2.type()],
      CurveType_name[promote_map.at(Ppair(obj1.type(),obj2.type()))]
    );

    switch ( promote_map.at(Ppair(obj1.type(),obj2.type())) ) {
    case G2LIB_LINE:
      {
        LineSegment L1( obj1 );
        LineSegment L2( obj2 );
        L1.intersect_ISO( offs1, L2, offs2, ilist, swap_s_vals );
      }
      break;
    case G2LIB_CIRCLE:
      {
        CircleArc C1( obj1 );
        CircleArc C2( obj2 );
        C1.intersect_ISO( offs1, C2, offs2, ilist, swap_s_vals );
      }
      break;
    case G2LIB_CLOTHOID:
      {
        ClothoidCurve C1( obj1 );
        ClothoidCurve C2( obj2 );
        C1.intersect_ISO( offs1, C2, offs2, ilist, swap_s_vals );
      }
      break;
    case G2LIB_BIARC:
      {
        Biarc B1( obj1 );
        Biarc B2( obj2 );
        B1.intersect_ISO( offs1, B2, offs2, ilist, swap_s_vals );
      }
      break;
    case G2LIB_BIARC_LIST:
      {
        BiarcList BL1( obj1 );
        BiarcList BL2( obj2 );
        BL1.intersect_ISO( offs1, BL2, offs2, ilist, swap_s_vals );
      }
      break;
    case G2LIB_CLOTHOID_LIST:
      {
        ClothoidList CL1( obj1 );
        ClothoidList CL2( obj2 );
        CL1.intersect_ISO( offs1, CL2, offs2, ilist, swap_s_vals );
      }
      break;
    case G2LIB_POLYLINE:
      {
        PolyLine PL1( obj1 );
        PolyLine PL2( obj2 );
        PL1.intersect_ISO( offs1, PL2, offs2, ilist, swap_s_vals );
      }
      break;
    }
  }
}

// EOF: G2lib_intersect.cc