LCOV - code coverage report
Current view: top level - src/common/autoware_auto_geometry/include/geometry - convex_hull.hpp (source / functions) Hit Total Coverage
Test: lcov.total.filtered Lines: 60 60 100.0 %
Date: 2021-01-26 05:02:50 Functions: 10 10 100.0 %
Legend: Lines: hit not hit | Branches: + taken - not taken # not executed Branches: 29 42 69.0 %

           Branch data     Line data    Source code
       1                 :            : // Copyright 2017-2019 the Autoware Foundation
       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 the monotone chain algorithm to compute 2D convex hulls on linked
      19                 :            : ///        lists of points
      20                 :            : 
      21                 :            : #ifndef GEOMETRY__CONVEX_HULL_HPP_
      22                 :            : #define GEOMETRY__CONVEX_HULL_HPP_
      23                 :            : 
      24                 :            : #include <common/types.hpp>
      25                 :            : #include <geometry/common_2d.hpp>
      26                 :            : 
      27                 :            : //lint -e537 NOLINT pclint vs cpplint
      28                 :            : #include <algorithm>
      29                 :            : //lint -e537 NOLINT pclint vs cpplint
      30                 :            : #include <list>
      31                 :            : #include <limits>
      32                 :            : #include <utility>
      33                 :            : 
      34                 :            : using autoware::common::types::float32_t;
      35                 :            : 
      36                 :            : namespace autoware
      37                 :            : {
      38                 :            : namespace common
      39                 :            : {
      40                 :            : namespace geometry
      41                 :            : {
      42                 :            : /// \brief Contains computation geometry functions not intended for the end user to directly use
      43                 :            : namespace details
      44                 :            : {
      45                 :            : 
      46                 :            : /// \brief Moves points comprising the lower convex hull from points to hull.
      47                 :            : /// \param[inout] points A list of points, assumed to be sorted in lexical order
      48                 :            : /// \param[inout] hull An empty list of points, assumed to have same allocator as points
      49                 :            : ///                    (for splice)
      50                 :            : /// \tparam PointT The point type
      51                 :            : template<typename PointT>
      52                 :         46 : void form_lower_hull(std::list<PointT> & points, std::list<PointT> & hull)
      53                 :            : {
      54                 :         46 :   auto hull_it = hull.cbegin();
      55                 :         46 :   auto point_it = points.cbegin();
      56                 :            :   // This ensures that the points we splice to back to the end of list are not processed
      57                 :         46 :   const auto iters = points.size();
      58         [ +  + ]:       2470 :   for (auto idx = decltype(iters) {0}; idx < iters; ++idx) {
      59                 :            :     // splice points from tail of hull to tail of list until point from head of list satisfies ccw
      60                 :       2424 :     bool8_t is_ccw = true;
      61   [ +  +  +  -  :       3686 :     while ((hull.cbegin() != hull_it) && is_ccw) {
                   +  + ]
      62                 :       3473 :       const auto current_hull_it = hull_it;
      63                 :       3473 :       --hull_it;
      64                 :       3473 :       is_ccw = ccw(*hull_it, *current_hull_it, *point_it);
      65         [ +  + ]:       3473 :       if (!is_ccw) {
      66                 :       2211 :         hull_it = current_hull_it;
      67                 :       2211 :         break;
      68                 :            :       }
      69                 :            :       // return this node to list for consideration in upper hull
      70                 :       1262 :       points.splice(points.end(), hull, current_hull_it);
      71                 :            :     }
      72                 :       2424 :     const auto last_point_it = point_it;
      73                 :       2424 :     ++point_it;
      74                 :            :     // Splice head of list to tail of (lower) hull
      75                 :       2424 :     hull.splice(hull.end(), points, last_point_it);
      76                 :            :     // point_it has been advanced, hull_it has been advanced (to where point_it was previously)
      77                 :       2424 :     hull_it = last_point_it;
      78                 :            :   }
      79                 :            :   // loop is guaranteed not to underflow because a node can be removed from points AT MOST once
      80                 :            :   // per loop iteration. The loop is upper bounded to the prior size of the point list
      81                 :         46 : }
      82                 :            : /// \brief Moves points comprising the lower convex hull from points to hull.
      83                 :            : /// \param[inout] points A list of points, assumed to be sorted in lexical order, and to contain
      84                 :            : ///                      the leftmost point
      85                 :            : /// \param[inout] hull An list of points, assumed to have same allocator as points (for splice),
      86                 :            : ///                    and to contain the lower hull (minus the left-most point)
      87                 :            : /// \tparam PointT The point type
      88                 :            : template<typename PointT>
      89                 :         46 : void form_upper_hull(std::list<PointT> & points, std::list<PointT> & hull)
      90                 :            : {
      91                 :            :   // TODO(c.ho) consider reverse iterators, not sure if they work with splice()
      92                 :         46 :   auto hull_it = hull.cend();
      93                 :         46 :   --hull_it;
      94                 :         46 :   const auto lower_hull_end = hull_it;
      95                 :         46 :   auto point_it = points.cend();
      96                 :         46 :   --point_it;
      97                 :            :   // This ensures that the points we splice to back to the head of list are not processed
      98                 :         46 :   const auto iters = points.size();
      99         [ +  + ]:       1354 :   for (auto idx = decltype(iters) {0}; idx < iters; ++idx) {
     100                 :            :     // splice points from tail of hull to head of list until ccw is satisfied with tail of list
     101                 :       1308 :     bool8_t is_ccw = true;
     102   [ +  +  +  -  :       1505 :     while ((lower_hull_end != hull_it) && is_ccw) {
                   +  + ]
     103                 :       1358 :       const auto current_hull_it = hull_it;
     104                 :       1358 :       --hull_it;
     105                 :       1358 :       is_ccw = ccw(*hull_it, *current_hull_it, *point_it);
     106         [ +  + ]:       1358 :       if (!is_ccw) {
     107                 :       1161 :         hull_it = current_hull_it;
     108                 :       1161 :         break;
     109                 :            :       }
     110                 :        197 :       points.splice(points.begin(), hull, current_hull_it);
     111                 :            :     }
     112                 :       1308 :     const auto last_point_it = point_it;
     113                 :       1308 :     --point_it;
     114                 :            :     // Splice points from tail of lexically ordered point list to tail of hull
     115                 :       1308 :     hull.splice(hull.end(), points, last_point_it);
     116                 :       1308 :     hull_it = last_point_it;
     117                 :            :   }
     118                 :            :   // loop is guaranteed not to underflow because a node can be removed from points AT MOST once
     119                 :            :   // per loop iteration. The loop is upper bounded to the prior size of the point list
     120                 :         46 : }
     121                 :            : 
     122                 :            : /// \brief A static memory implementation of convex hull computation. Shuffles points around the
     123                 :            : ///        deque such that the points of the convex hull of the deque of points are first in the
     124                 :            : ///        deque, with the internal points following in an unspecified order.
     125                 :            : ///        The head of the deque will be the point with the smallest x value, with the other
     126                 :            : ///        points following in a counter-clockwise manner (from a top down view/facing -z direction)
     127                 :            : /// \param[inout] list A list of nodes that will be pruned down and reordered into a ccw convex hull
     128                 :            : /// \return An iterator pointing to one after the last point contained in the hull
     129                 :            : /// \tparam PointT Type of a point, must have x and y float members
     130                 :            : template<typename PointT>
     131                 :         46 : typename std::list<PointT>::const_iterator convex_hull_impl(std::list<PointT> & list)
     132                 :            : {
     133                 :            :   // Functor that return whether a <= b in the lexical sense (a.x < b.x), sort by y if tied
     134                 :      17259 :   const auto lexical_comparator = [](const PointT & a, const PointT & b) -> bool8_t
     135                 :            :     {
     136                 :            :       using point_adapter::x_;
     137                 :            :       using point_adapter::y_;
     138                 :      17259 :       constexpr auto FEPS = std::numeric_limits<float32_t>::epsilon();
     139   [ +  +  +  + ]:      34518 :       return (fabsf(x_(a) - x_(b)) > FEPS) ?
     140                 :      34518 :              (x_(a) < x_(b)) : (y_(a) < y_(b));
     141                 :            :     };
     142         [ +  - ]:         46 :   list.sort(lexical_comparator);
     143                 :            : 
     144                 :            :   // Temporary list to store points
     145                 :         46 :   std::list<PointT> tmp_hull_list{list.get_allocator()};
     146                 :            : 
     147                 :            :   // Shuffle lower hull points over to tmp_hull_list
     148         [ +  - ]:         46 :   form_lower_hull(list, tmp_hull_list);
     149                 :            : 
     150                 :            :   // Resort list since we can't guarantee the order TODO(c.ho) is this true?
     151         [ +  - ]:         46 :   list.sort(lexical_comparator);
     152                 :            :   // splice first hull point to beginning of list to ensure upper hull is properly closed
     153                 :            :   // This is done after sorting because we know exactly where it should go, and keeping it out of
     154                 :            :   // sorting reduces complexity somewhat
     155                 :         46 :   list.splice(list.begin(), tmp_hull_list, tmp_hull_list.begin());
     156                 :            : 
     157                 :            :   // build upper hull
     158         [ +  - ]:         46 :   form_upper_hull(list, tmp_hull_list);
     159                 :            :   // Move hull to beginning of list, return iterator pointing to one after the convex hull
     160                 :         46 :   const auto ret = list.begin();
     161                 :            :   // first move left-most point to ensure it is first
     162                 :         46 :   auto tmp_it = tmp_hull_list.end();
     163                 :         46 :   --tmp_it;
     164         [ #  # ]:         46 :   list.splice(list.begin(), tmp_hull_list, tmp_it);
     165                 :            :   // Then move remainder of hull points
     166                 :         46 :   list.splice(ret, tmp_hull_list);
     167                 :         92 :   return ret;
     168                 :            : }
     169                 :            : }  // namespace details
     170                 :            : 
     171                 :            : /// \brief A static memory implementation of convex hull computation. Shuffles points around the
     172                 :            : ///        deque such that the points of the convex hull of the deque of points are first in the
     173                 :            : ///        deque, with the internal points following in an unspecified order.
     174                 :            : ///
     175                 :            : ///        The head of the deque will be the point with the smallest x value, with the other
     176                 :            : ///        points following in a counter-clockwise manner (from a top down view/facing -z
     177                 :            : ///        direction). If the point list is 3 or smaller, nothing is done (e.g. the ordering result
     178                 :            : ///        as previously stated does not hold).
     179                 :            : /// \param[inout] list A list of nodes that will be reordered into a ccw convex hull
     180                 :            : /// \return An iterator pointing to one after the last point contained in the hull
     181                 :            : /// \tparam PointT Type of a point, must have x and y float members
     182                 :            : template<typename PointT>
     183                 :         52 : typename std::list<PointT>::const_iterator convex_hull(std::list<PointT> & list)
     184                 :            : {
     185   [ +  +  +  -  :        104 :   return (list.size() <= 3U) ? list.end() : details::convex_hull_impl(list);
             #  #  #  # ]
     186                 :            : }
     187                 :            : 
     188                 :            : }  // namespace geometry
     189                 :            : }  // namespace common
     190                 :            : }  // namespace autoware
     191                 :            : 
     192                 :            : #endif  // GEOMETRY__CONVEX_HULL_HPP_

Generated by: LCOV version 1.14