hull_pockets.hpp

Go to the documentation of this file.

// Copyright 2020 Embotech AG, Zurich, Switzerland. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
 
#ifndef GEOMETRY__HULL_POCKETS_HPP_
#define GEOMETRY__HULL_POCKETS_HPP_
 
#include <common/types.hpp>
#include <geometry/common_2d.hpp>
#include <vector>
#include <utility>
#include <limits>
#include <algorithm>
#include <iterator>
 
using autoware::common::types::float32_t;
 
namespace autoware
{
namespace common
{
namespace geometry
{
 
 
//    * "Rollover" is not properly handled - if a pocket contains the segment from
//      the last point in the list to the first point (which is still part of the
//      polygon), that does not get added
template<typename Iter1, typename Iter2>
typename std::vector<std::vector<typename std::iterator_traits<Iter1>::value_type>> hull_pockets(
  const Iter1 polygon_start,
  const Iter1 polygon_end,
  const Iter2 convex_hull_start,
  const Iter2 convex_hull_end
)
{
  auto pockets = std::vector<std::vector<typename std::iterator_traits<Iter1>::value_type>>{};
  if (std::distance(polygon_start, polygon_end) <= 3) {
    return pockets;
  }
 
  // Function to check whether a point is in the convex hull.
  const auto in_convex_hull = [convex_hull_start, convex_hull_end](Iter1 p) {
      return std::any_of(
        convex_hull_start, convex_hull_end, [p](auto hull_entry)
        {
          return norm_2d(
            minus_2d(
              hull_entry,
              *p)) < std::numeric_limits<float32_t>::epsilon();
        });
    };
 
  // We go through the points of the polygon only once, adding pockets to the list of pockets
  // as we detect them.
  std::vector<typename std::iterator_traits<Iter1>::value_type> current_pocket{};
  for (auto it = polygon_start; it != polygon_end; it = std::next(it)) {
    if (in_convex_hull(it)) {
      if (current_pocket.size() > 1) {
        current_pocket.emplace_back(*it);
        pockets.push_back(current_pocket);
      }
      current_pocket.clear();
      current_pocket.emplace_back(*it);
    } else {
      current_pocket.emplace_back(*it);
    }
  }
 
  // At this point, we have reached the end of the polygon, but have not considered the connection
  // of the final point back to the first. In case the first point is part of a pocket as well, we
  // have some points left in current_pocket, and we add one final pocket that is made up by those
  // points plus the first point in the collection.
  if (current_pocket.size() > 1) {
    current_pocket.push_back(*polygon_start);
    pockets.push_back(current_pocket);
  }
 
  return pockets;
}
}  // namespace geometry
}  // namespace common
}  // namespace autoware
 
#endif  // GEOMETRY__HULL_POCKETS_HPP_