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                 :            : // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
      16                 :            : 
      17                 :            : /// \file
      18                 :            : /// \brief This file implements an algorithm for getting a list of "pockets" in the convex
      19                 :            : ///        hull of a non-convex simple polygon.
      20                 :            : 
      21                 :            : #ifndef GEOMETRY__HULL_POCKETS_HPP_
      22                 :            : #define GEOMETRY__HULL_POCKETS_HPP_
      23                 :            : 
      24                 :            : #include <common/types.hpp>
      25                 :            : #include <geometry/common_2d.hpp>
      26                 :            : #include <vector>
      27                 :            : #include <utility>
      28                 :            : #include <limits>
      29                 :            : #include <algorithm>
      30                 :            : #include <iterator>
      31                 :            : 
      32                 :            : using autoware::common::types::float32_t;
      33                 :            : 
      34                 :            : namespace autoware
      35                 :            : {
      36                 :            : namespace common
      37                 :            : {
      38                 :            : namespace geometry
      39                 :            : {
      40                 :            : 
      41                 :            : 
      42                 :            : /// \brief Compute a list of "pockets" of a simple polygon
      43                 :            : ///   (https://en.wikipedia.org/wiki/Simple_polygon), that is, the areas that make
      44                 :            : ///   up the difference between the polygon and its convex hull.
      45                 :            : ///   This currently has a bug:
      46                 :            : //    * "Rollover" is not properly handled - if a pocket contains the segment from
      47                 :            : //      the last point in the list to the first point (which is still part of the
      48                 :            : //      polygon), that does not get added
      49                 :            : ///
      50                 :            : /// \param[in] polygon_start Start iterator for the simple polygon (has to be on convex hull)
      51                 :            : /// \param[in] polygon_end End iterator for the simple polygon
      52                 :            : /// \param[in] convex_hull_start Start iterator for the convex hull of the simple polygon
      53                 :            : /// \param[in] convex_hull_end End iterator for the convex hull of the simple polygon
      54                 :            : /// \return A vector of vectors of the iterator value type. Each inner vector contains the points
      55                 :            : ///         for one pocket. We return by value instead of as iterator pairs, because it is possible
      56                 :            : ///         that the edge connecting the final point in the list and the first point in the list is
      57                 :            : ///         part of a pocket as well, and this is awkward to represent using iterators into the
      58                 :            : ///         original collection.
      59                 :            : ///
      60                 :            : /// \tparam Iter1 Iterator to a point type
      61                 :            : /// \tparam Iter2 Iterator to a point type (can be the same as Iter1, but does not need to be)
      62                 :            : template<typename Iter1, typename Iter2>
      63                 :          0 : typename std::vector<std::vector<typename std::iterator_traits<Iter1>::value_type>> hull_pockets(
      64                 :            :   const Iter1 polygon_start,
      65                 :            :   const Iter1 polygon_end,
      66                 :            :   const Iter2 convex_hull_start,
      67                 :            :   const Iter2 convex_hull_end
      68                 :            : )
      69                 :            : {
      70                 :          0 :   auto pockets = std::vector<std::vector<typename std::iterator_traits<Iter1>::value_type>>{};
      71                 :          0 :   if (std::distance(polygon_start, polygon_end) <= 3) {
      72                 :          0 :     return pockets;
      73                 :            :   }
      74                 :            : 
      75                 :            :   // Function to check whether a point is in the convex hull.
      76                 :          0 :   const auto in_convex_hull = [convex_hull_start, convex_hull_end](Iter1 p) {
      77                 :          0 :       return std::any_of(
      78                 :          0 :         convex_hull_start, convex_hull_end, [p](auto hull_entry)
      79                 :            :         {
      80                 :          0 :           return norm_2d(
      81                 :          0 :             minus_2d(
      82                 :            :               hull_entry,
      83                 :          0 :               *p)) < std::numeric_limits<float32_t>::epsilon();
      84                 :          0 :         });
      85                 :            :     };
      86                 :            : 
      87                 :            :   // We go through the points of the polygon only once, adding pockets to the list of pockets
      88                 :            :   // as we detect them.
      89                 :          0 :   std::vector<typename std::iterator_traits<Iter1>::value_type> current_pocket{};
      90                 :          0 :   for (auto it = polygon_start; it != polygon_end; it = std::next(it)) {
      91                 :          0 :     if (in_convex_hull(it)) {
      92                 :          0 :       if (current_pocket.size() > 1) {
      93                 :          0 :         current_pocket.emplace_back(*it);
      94                 :          0 :         pockets.push_back(current_pocket);
      95                 :            :       }
      96                 :          0 :       current_pocket.clear();
      97                 :          0 :       current_pocket.emplace_back(*it);
      98                 :            :     } else {
      99                 :          0 :       current_pocket.emplace_back(*it);
     100                 :            :     }
     101                 :            :   }
     102                 :            : 
     103                 :            :   // At this point, we have reached the end of the polygon, but have not considered the connection
     104                 :            :   // of the final point back to the first. In case the first point is part of a pocket as well, we
     105                 :            :   // have some points left in current_pocket, and we add one final pocket that is made up by those
     106                 :            :   // points plus the first point in the collection.
     107                 :          0 :   if (current_pocket.size() > 1) {
     108                 :          0 :     current_pocket.push_back(*polygon_start);
     109                 :          0 :     pockets.push_back(current_pocket);
     110                 :            :   }
     111                 :            : 
     112                 :          0 :   return pockets;
     113                 :            : }
     114                 :            : }  // namespace geometry
     115                 :            : }  // namespace common
     116                 :            : }  // namespace autoware
     117                 :            : 
     118                 :            : #endif  // GEOMETRY__HULL_POCKETS_HPP_