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: 28 28 100.0 %
Date: 2023-03-03 05:44:19 Functions: 9 9 100.0 %
Legend: Lines: hit not hit | Branches: + taken - not taken # not executed Branches: 32 40 80.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 for the points list
      51                 :            : /// \tparam HullT the point type for the hull list
      52                 :            : template<typename PointT, typename HullT>
      53                 :         70 : void form_lower_hull(std::list<PointT> & points, std::list<HullT> & hull)
      54                 :            : {
      55                 :         70 :   auto hull_it = hull.cbegin();
      56                 :         70 :   auto point_it = points.cbegin();
      57                 :            :   // This ensures that the points we splice to back to the end of list are not processed
      58                 :            :   const auto iters = points.size();
      59         [ +  + ]:       2597 :   for (auto idx = decltype(iters) {0}; idx < iters; ++idx) {
      60                 :            :     // splice points from tail of hull to tail of list until point from head of list satisfies ccw
      61                 :            :     bool8_t is_ccw = true;
      62   [ +  +  +  - ]:       3817 :     while ((hull.cbegin() != hull_it) && is_ccw) {
      63                 :            :       const auto current_hull_it = hull_it;
      64                 :            :       --hull_it;
      65                 :            :       is_ccw = ccw(*hull_it, *current_hull_it, *point_it);
      66         [ +  + ]:       3535 :       if (!is_ccw) {
      67                 :            :         hull_it = current_hull_it;
      68                 :            :         break;
      69                 :            :       }
      70                 :            :       // return this node to list for consideration in upper hull
      71                 :       1290 :       points.splice(points.end(), hull, current_hull_it);
      72                 :            :     }
      73                 :            :     const auto last_point_it = point_it;
      74                 :            :     ++point_it;
      75                 :            :     // Splice head of list to tail of (lower) hull
      76                 :       2527 :     hull.splice(hull.end(), points, last_point_it);
      77                 :            :     // point_it has been advanced, hull_it has been advanced (to where point_it was previously)
      78                 :            :     hull_it = last_point_it;
      79                 :            :   }
      80                 :            :   // loop is guaranteed not to underflow because a node can be removed from points AT MOST once
      81                 :            :   // per loop iteration. The loop is upper bounded to the prior size of the point list
      82                 :         70 : }
      83                 :            : /// \brief Moves points comprising the lower convex hull from points to hull.
      84                 :            : /// \param[inout] points A list of points, assumed to be sorted in lexical order, and to contain
      85                 :            : ///                      the leftmost point
      86                 :            : /// \param[inout] hull A list of points, assumed to have same allocator as points (for splice),
      87                 :            : ///                    and to contain the lower hull (minus the left-most point)
      88                 :            : /// \tparam PointT The point type for the points list
      89                 :            : /// \tparam HullT the point type for the hull list
      90                 :            : template<typename PointT, typename HullT>
      91                 :         70 : void form_upper_hull(std::list<PointT> & points, std::list<HullT> & hull)
      92                 :            : {
      93                 :            :   // TODO(c.ho) consider reverse iterators, not sure if they work with splice()
      94                 :            :   auto hull_it = hull.cend();
      95                 :            :   --hull_it;
      96                 :            :   const auto lower_hull_end = hull_it;
      97                 :            :   auto point_it = points.cend();
      98                 :            :   --point_it;
      99                 :            :   // This ensures that the points we splice to back to the head of list are not processed
     100                 :            :   const auto iters = points.size();
     101         [ +  + ]:       1430 :   for (auto idx = decltype(iters) {0}; idx < iters; ++idx) {
     102                 :            :     // splice points from tail of hull to head of list until ccw is satisfied with tail of list
     103                 :            :     bool8_t is_ccw = true;
     104   [ +  +  +  - ]:       1561 :     while ((lower_hull_end != hull_it) && is_ccw) {
     105                 :            :       const auto current_hull_it = hull_it;
     106                 :            :       --hull_it;
     107                 :            :       is_ccw = ccw(*hull_it, *current_hull_it, *point_it);
     108         [ +  + ]:       1386 :       if (!is_ccw) {
     109                 :            :         hull_it = current_hull_it;
     110                 :            :         break;
     111                 :            :       }
     112                 :        201 :       points.splice(points.begin(), hull, current_hull_it);
     113                 :            :     }
     114                 :            :     const auto last_point_it = point_it;
     115                 :            :     --point_it;
     116                 :            :     // Splice points from tail of lexically ordered point list to tail of hull
     117                 :       1360 :     hull.splice(hull.end(), points, last_point_it);
     118                 :            :     hull_it = last_point_it;
     119                 :            :   }
     120                 :            :   // loop is guaranteed not to underflow because a node can be removed from points AT MOST once
     121                 :            :   // per loop iteration. The loop is upper bounded to the prior size of the point list
     122                 :         70 : }
     123                 :            : 
     124                 :            : /// \brief A static memory implementation of convex hull computation. Shuffles points around the
     125                 :            : ///        deque such that the points of the convex hull of the deque of points are first in the
     126                 :            : ///        deque, with the internal points following in an unspecified order.
     127                 :            : ///        The head of the deque will be the point with the smallest x value, with the other
     128                 :            : ///        points following in a counter-clockwise manner (from a top down view/facing -z direction)
     129                 :            : /// \param[inout] list A list of nodes that will be pruned down and reordered into a ccw convex hull
     130                 :            : /// \return An iterator pointing to one after the last point contained in the hull
     131                 :            : /// \tparam PointT Type of a point, must have x and y float members
     132                 :            : template<typename PointT>
     133                 :         70 : typename std::list<PointT>::const_iterator convex_hull_impl(std::list<PointT> & list)
     134                 :            : {
     135                 :            :   // Functor that return whether a <= b in the lexical sense (a.x < b.x), sort by y if tied
     136                 :            :   const auto lexical_comparator = [](const PointT & a, const PointT & b) -> bool8_t
     137                 :            :     {
     138                 :            :       using point_adapter::x_;
     139                 :            :       using point_adapter::y_;
     140                 :            :       constexpr auto FEPS = std::numeric_limits<float32_t>::epsilon();
     141   [ +  +  +  + ]:      17397 :       return (fabsf(x_(a) - x_(b)) > FEPS) ?
     142                 :        590 :              (x_(a) < x_(b)) : (y_(a) < y_(b));
     143                 :            :     };
     144                 :         70 :   list.sort(lexical_comparator);
     145                 :            : 
     146                 :            :   // Temporary list to store points
     147                 :            :   std::list<PointT> tmp_hull_list{list.get_allocator()};
     148                 :            : 
     149                 :            :   // Shuffle lower hull points over to tmp_hull_list
     150                 :         70 :   form_lower_hull(list, tmp_hull_list);
     151                 :            : 
     152                 :            :   // Resort list since we can't guarantee the order TODO(c.ho) is this true?
     153                 :         70 :   list.sort(lexical_comparator);
     154                 :            :   // splice first hull point to beginning of list to ensure upper hull is properly closed
     155                 :            :   // This is done after sorting because we know exactly where it should go, and keeping it out of
     156                 :            :   // sorting reduces complexity somewhat
     157                 :         70 :   list.splice(list.begin(), tmp_hull_list, tmp_hull_list.begin());
     158                 :            : 
     159                 :            :   // build upper hull
     160                 :         70 :   form_upper_hull(list, tmp_hull_list);
     161                 :            :   // Move hull to beginning of list, return iterator pointing to one after the convex hull
     162                 :         70 :   const auto ret = list.begin();
     163                 :            :   // first move left-most point to ensure it is first
     164                 :            :   auto tmp_it = tmp_hull_list.end();
     165                 :            :   --tmp_it;
     166         [ +  - ]:         70 :   list.splice(list.begin(), tmp_hull_list, tmp_it);
     167                 :            :   // Then move remainder of hull points
     168                 :            :   list.splice(ret, tmp_hull_list);
     169                 :         70 :   return ret;
     170                 :            : }
     171                 :            : }  // namespace details
     172                 :            : 
     173                 :            : /// \brief A static memory implementation of convex hull computation. Shuffles points around the
     174                 :            : ///        deque such that the points of the convex hull of the deque of points are first in the
     175                 :            : ///        deque, with the internal points following in an unspecified order.
     176                 :            : ///
     177                 :            : ///        The head of the deque will be the point with the smallest x value, with the other
     178                 :            : ///        points following in a counter-clockwise manner (from a top down view/facing -z
     179                 :            : ///        direction). If the point list is 3 or smaller, nothing is done (e.g. the ordering result
     180                 :            : ///        as previously stated does not hold).
     181                 :            : /// \param[inout] list A list of nodes that will be reordered into a ccw convex hull
     182                 :            : /// \return An iterator pointing to one after the last point contained in the hull
     183                 :            : /// \tparam PointT Type of a point, must have x and y float members
     184                 :            : template<typename PointT>
     185                 :            : typename std::list<PointT>::const_iterator convex_hull(std::list<PointT> & list)
     186                 :            : {
     187   [ +  +  +  +  :         89 :   return (list.size() <= 3U) ? list.end() : details::convex_hull_impl(list);
          +  +  +  +  -  
          +  -  +  -  +  
             -  +  -  + ]
     188                 :            : }
     189                 :            : 
     190                 :            : }  // namespace geometry
     191                 :            : }  // namespace common
     192                 :            : }  // namespace autoware
     193                 :            : 
     194                 :            : #endif  // GEOMETRY__CONVEX_HULL_HPP_

Generated by: LCOV version 1.14