# Program Listing for File AABBtree.hxx

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

    /*--------------------------------------------------------------------------*\
     |                                                                          |
     |  Copyright (C) 2018                                                      |
     |                                                                          |
     |         , __                 , __                                        |
     |        /|/  \               /|/  \                                       |
     |         | __/ _   ,_         | __/ _   ,_                                |
     |         |   \|/  /  |  |   | |   \|/  /  |  |   |                        |
     |         |(__/|__/   |_/ \_/|/|(__/|__/   |_/ \_/|/                       |
     |                           /|                   /|                        |
     |                           \|                   \|                        |
     |                                                                          |
     |      Paolo Bevilacqua and Enrico Bertolazzi                              |
     |                                                                          |
     |      (1) Dipartimento di Ingegneria e Scienza dell'Informazione          |
     |      (2) Dipartimento di Ingegneria Industriale                          |
     |                                                                          |
     |      Universita` degli Studi di Trento                                   |
     |      email: paolo.bevilacqua@unitn.it                                    |
     |      email: enrico.bertolazzi@unitn.it                                   |
     |                                                                          |
    \*--------------------------------------------------------------------------*/

    namespace G2lib {

      using std::setw;
      using std::vector;
      using std::pair;

      #ifdef G2LIB_USE_CXX11
      using std::make_shared;
      using std::shared_ptr; // promemoria shared_ptr<Foo>(&foo, [](void*){});
      #endif

      class AABBtree;

      /*\
       |   ____  ____
       |  | __ )| __ )  _____  __
       |  |  _ \|  _ \ / _ \ \/ /
       |  | |_) | |_) | (_) >  <
       |  |____/|____/ \___/_/\_\
      \*/
      class BBox {
      public:
        #ifdef G2LIB_USE_CXX11
        typedef shared_ptr<BBox const> PtrBBox;
        #else
        typedef BBox const * PtrBBox;
        #endif

      private:
        real_type m_xmin;
        real_type m_ymin;
        real_type m_xmax;
        real_type m_ymax;
        int_type  m_id;
        int_type  m_ipos;
        BBox();
        BBox( BBox const & );

      public:

        BBox(
          real_type xmin,
          real_type ymin,
          real_type xmax,
          real_type ymax,
          int_type  id,
          int_type  ipos
        ) {
          m_xmin = xmin;
          m_ymin = ymin;
          m_xmax = xmax;
          m_ymax = ymax;
          m_id   = id;
          m_ipos = ipos;
        }

        BBox(
          vector<PtrBBox> const & bboxes,
          int_type                id,
          int_type                ipos
        ) {
          m_id   = id;
          m_ipos = ipos;
          this -> join( bboxes );
        }

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

        real_type Xmin() const { return m_xmin; }
        real_type Ymin() const { return m_ymin; }
        real_type Xmax() const { return m_xmax; }
        real_type Ymax() const { return m_ymax; }

        int_type const & Id()   const { return m_id; }
        int_type const & Ipos() const { return m_ipos; }

        BBox const &
        operator = ( BBox const & rhs ) {
          m_xmin = rhs.m_xmin;
          m_ymin = rhs.m_ymin;
          m_xmax = rhs.m_xmax;
          m_ymax = rhs.m_ymax;
          m_id   = rhs.m_id;
          m_ipos = rhs.m_ipos;
          return *this;
        }

        bool
        collision( BBox const & box ) const {
          return !( (box.m_xmin > m_xmax ) ||
                    (box.m_xmax < m_xmin ) ||
                    (box.m_ymin > m_ymax ) ||
                    (box.m_ymax < m_ymin ) );
        }

        void
        join( vector<PtrBBox> const & bboxes );

        real_type
        distance( real_type x, real_type y ) const;

        real_type
        maxDistance( real_type x, real_type y ) const;

        void
        print( ostream_type & stream ) const {
          fmt::print( stream,
            "BBOX (xmin,ymin,xmax,ymax) = ( {}, {}, {}, {} )\n",
            m_xmin, m_ymin, m_xmax, m_ymax
          );
        }

        friend class AABBtree;
      };

      inline
      ostream_type &
      operator << ( ostream_type & stream, BBox const & bb ) {
        bb.print(stream);
        return stream;
      }

      /*\
       |      _        _    ____  ____  _
       |     / \      / \  | __ )| __ )| |_ _ __ ___  ___
       |    / _ \    / _ \ |  _ \|  _ \| __| '__/ _ \/ _ \
       |   / ___ \  / ___ \| |_) | |_) | |_| | |  __/  __/
       |  /_/   \_\/_/   \_\____/|____/ \__|_|  \___|\___|
      \*/
      class AABBtree {
      public:

      #ifdef G2LIB_USE_CXX11
        typedef shared_ptr<BBox const> PtrBBox;
        typedef shared_ptr<AABBtree>   PtrAABB;
      #else
        typedef BBox const *           PtrBBox;
        typedef AABBtree *             PtrAABB;
      #endif

      typedef pair<PtrBBox,PtrBBox> PairPtrBBox;
      typedef vector<PtrBBox>       VecPtrBBox;
      typedef vector<PairPtrBBox>   VecPairPtrBBox;

      private:

        // bbox of the tree
        PtrBBox         pBBox;
        vector<PtrAABB> children;

        AABBtree( AABBtree const & tree );

        static
        real_type
        min_maxdist(
          real_type        x,
          real_type        y,
          AABBtree const & tree,
          real_type        mmDist
        );

        static
        void
        min_maxdist_select(
          real_type        x,
          real_type        y,
          real_type        mmDist,
          AABBtree const & tree,
          VecPtrBBox     & candidateList
        );

      public:

        AABBtree();

        ~AABBtree();

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

        void clear();

        bool empty() const;

        void
        bbox(
          real_type & xmin,
          real_type & ymin,
          real_type & xmax,
          real_type & ymax
        ) const {
          xmin = pBBox->m_xmin;
          ymin = pBBox->m_ymin;
          xmax = pBBox->m_xmax;
          ymax = pBBox->m_ymax;
        }

        void
        build( vector<PtrBBox> const & bboxes );

        void
        print( ostream_type & stream, int level = 0 ) const;

        template <typename COLLISION_fun>
        bool
        collision(
          AABBtree const & tree,
          COLLISION_fun    ifun,
          bool             swap_tree = false
        ) const {

          // check bbox with
          if ( !tree.pBBox->collision(*pBBox) ) return false;

          int icase = (children.empty() ? 0 : 1) +
                      (tree.children.empty()? 0 : 2);

          switch ( icase ) {
          case 0: // both leaf, use GeomPrimitive intersection algorithm
            if ( swap_tree ) return ifun( tree.pBBox, pBBox );
            else             return ifun( pBBox, tree.pBBox );
          case 1: // first is a tree, second is a leaf
            { typename vector<PtrAABB>::const_iterator it;
              for ( it = children.begin(); it != children.end(); ++it )
                if ( tree.collision( **it, ifun, !swap_tree ) )
                  return true;
            }
            break;
          case 2: // first leaf, second is a tree
            { typename vector<PtrAABB>::const_iterator it;
              for ( it = tree.children.begin();
                    it != tree.children.end(); ++it )
                if ( this->collision( **it, ifun, swap_tree ) )
                  return true;
            }
            break;
          case 3: // first is a tree, second is a tree
            { typename vector<PtrAABB>::const_iterator c1;
              typename vector<PtrAABB>::const_iterator c2;
              for ( c1 = children.begin(); c1 != children.end(); ++c1 )
                for ( c2 = tree.children.begin();
                      c2 != tree.children.end(); ++c2 )
                  if ( (*c1)->collision( **c2, ifun, swap_tree ) )
                    return true;
            }
            break;
          }
          return false;
        }

        void
        intersect(
          AABBtree const & tree,
          VecPairPtrBBox & intersectionList,
          bool             swap_tree = false
        ) const;

        void
        min_distance(
          real_type    x,
          real_type    y,
          VecPtrBBox & candidateList
        ) const;

      };

    }