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_