Branch data     Line data    Source code

       1                 :            : // Copyright 2020 Embotech AG, Zurich, Switzerland. All rights reserved.
       2                 :            : //
       3                 :            : // Licensed under the Apache License, Version 2.0 (the "License");
       4                 :            : // you may not use this file except in compliance with the License.
       5                 :            : // You may obtain a copy of the License at
       6                 :            : //
       7                 :            : //     http://www.apache.org/licenses/LICENSE-2.0
       8                 :            : //
       9                 :            : // Unless required by applicable law or agreed to in writing, software
      10                 :            : // distributed under the License is distributed on an "AS IS" BASIS,
      11                 :            : // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
      12                 :            : // See the License for the specific language governing permissions and
      13                 :            : // limitations under the License.
      14                 :            : 
      15                 :            : /// \file
      16                 :            : /// \brief This file implements an algorithm for getting a list of "pockets" in the convex
      17                 :            : ///        hull of a non-convex simple polygon.
      18                 :            : 
      19                 :            : #ifndef GEOMETRY__HULL_POCKETS_HPP_
      20                 :            : #define GEOMETRY__HULL_POCKETS_HPP_
      21                 :            : 
      22                 :            : #include <common/types.hpp>
      23                 :            : #include <geometry/common_2d.hpp>
      24                 :            : #include <vector>
      25                 :            : #include <utility>
      26                 :            : #include <limits>
      27                 :            : #include <algorithm>
      28                 :            : #include <iterator>
      29                 :            : 
      30                 :            : using autoware::common::types::float32_t;
      31                 :            : 
      32                 :            : namespace autoware
      33                 :            : {
      34                 :            : namespace common
      35                 :            : {
      36                 :            : namespace geometry
      37                 :            : {
      38                 :            : 
      39                 :            : 
      40                 :            : /// \brief Compute a list of "pockets" of a simple polygon
      41                 :            : ///   (https://en.wikipedia.org/wiki/Simple_polygon), that is, the areas that make
      42                 :            : ///   up the difference between the polygon and its convex hull.
      43                 :            : ///   This currently has a bug:
      44                 :            : //    * "Rollover" is not properly handled - if a pocket contains the segment from
      45                 :            : //      the last point in the list to the first point (which is still part of the
      46                 :            : //      polygon), that does not get added
      47                 :            : ///
      48                 :            : /// \param[in] polygon_start Start iterator for the simple polygon (has to be on convex hull)
      49                 :            : /// \param[in] polygon_end End iterator for the simple polygon
      50                 :            : /// \param[in] convex_hull_start Start iterator for the convex hull of the simple polygon
      51                 :            : /// \param[in] convex_hull_end End iterator for the convex hull of the simple polygon
      52                 :            : /// \return A vector of vectors of the iterator value type. Each inner vector contains the points
      53                 :            : ///         for one pocket. We return by value instead of as iterator pairs, because it is possible
      54                 :            : ///         that the edge connecting the final point in the list and the first point in the list is
      55                 :            : ///         part of a pocket as well, and this is awkward to represent using iterators into the
      56                 :            : ///         original collection.
      57                 :            : ///
      58                 :            : /// \tparam Iter1 Iterator to a point type
      59                 :            : /// \tparam Iter2 Iterator to a point type (can be the same as Iter1, but does not need to be)
      60                 :            : template<typename Iter1, typename Iter2>
      61                 :          5 : typename std::vector<std::vector<typename std::iterator_traits<Iter1>::value_type>> hull_pockets(
      62                 :            :   const Iter1 polygon_start,
      63                 :            :   const Iter1 polygon_end,
      64                 :            :   const Iter2 convex_hull_start,
      65                 :            :   const Iter2 convex_hull_end
      66                 :            : )
      67                 :            : {
      68                 :          5 :   auto pockets = std::vector<std::vector<typename std::iterator_traits<Iter1>::value_type>>{};
      69   [ +  -  +  + ]:          5 :   if (std::distance(polygon_start, polygon_end) <= 3) {
      70                 :          1 :     return pockets;
      71                 :            :   }
      72                 :            : 
      73                 :            :   // Function to check whether a point is in the convex hull.
      74                 :         29 :   const auto in_convex_hull = [convex_hull_start, convex_hull_end](Iter1 p) {
      75                 :         25 :       return std::any_of(
      76                 :         90 :         convex_hull_start, convex_hull_end, [p](auto hull_entry)
      77                 :            :         {
      78         [ +  - ]:         90 :           return norm_2d(
      79                 :         90 :             minus_2d(
      80                 :            :               hull_entry,
      81                 :         90 :               *p)) < std::numeric_limits<float32_t>::epsilon();
      82                 :         25 :         });
      83                 :            :     };
      84                 :            : 
      85                 :            :   // We go through the points of the polygon only once, adding pockets to the list of pockets
      86                 :            :   // as we detect them.
      87                 :          8 :   std::vector<typename std::iterator_traits<Iter1>::value_type> current_pocket{};
      88   [ +  +  +  - ]:         29 :   for (auto it = polygon_start; it != polygon_end; it = std::next(it)) {
      89   [ +  -  +  + ]:         25 :     if (in_convex_hull(it)) {
      90         [ +  + ]:         20 :       if (current_pocket.size() > 1) {
      91         [ +  - ]:          3 :         current_pocket.emplace_back(*it);
      92         [ +  - ]:          3 :         pockets.push_back(current_pocket);
      93                 :            :       }
      94                 :         20 :       current_pocket.clear();
      95         [ +  - ]:         20 :       current_pocket.emplace_back(*it);
      96                 :            :     } else {
      97         [ +  - ]:          5 :       current_pocket.emplace_back(*it);
      98                 :            :     }
      99                 :            :   }
     100                 :            : 
     101                 :            :   // At this point, we have reached the end of the polygon, but have not considered the connection
     102                 :            :   // of the final point back to the first. In case the first point is part of a pocket as well, we
     103                 :            :   // have some points left in current_pocket, and we add one final pocket that is made up by those
     104                 :            :   // points plus the first point in the collection.
     105         [ +  + ]:          4 :   if (current_pocket.size() > 1) {
     106         [ +  - ]:          1 :     current_pocket.push_back(*polygon_start);
     107         [ +  - ]:          1 :     pockets.push_back(current_pocket);
     108                 :            :   }
     109                 :            : 
     110                 :          4 :   return pockets;
     111                 :            : }
     112                 :            : }  // namespace geometry
     113                 :            : }  // namespace common
     114                 :            : }  // namespace autoware
     115                 :            : 
     116                 :            : #endif  // GEOMETRY__HULL_POCKETS_HPP_