polygon_fill.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018, Locus Robotics
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
37 #include <nav_2d_utils/polygons.h>
38 #include <algorithm>
39 
40 namespace nav_grid_iterators
41 {
42 PolygonFill::PolygonFill(const nav_grid::NavGridInfo* info, nav_2d_msgs::Polygon2D polygon)
43  : BaseIterator(info), polygon_(polygon), start_index_(0, 0)
44 {
45  if (polygon.points.size() == 0)
46  {
47  internal_iterator_.reset(new SubGrid(info_, 0, 0, 0, 0));
50  return;
51  }
52 
53  // Find the minimum and maximum coordinates of the vertices
54  double min_x = polygon_.points[0].x;
55  double max_x = min_x;
56  double min_y = polygon_.points[0].y;
57  double max_y = min_y;
58  for (const auto& vertex : polygon_.points)
59  {
60  min_x = std::min(min_x, vertex.x);
61  min_y = std::min(min_y, vertex.y);
62  max_x = std::max(max_x, vertex.x);
63  max_y = std::max(max_y, vertex.y);
64  }
65 
66  // Save the minimum in grid coordinates
67  worldToGridBounded(*info_, min_x, min_y, min_x_, min_y_);
68 
69  // Calculate the maximum in grid coordinates and then save the width/height
70  unsigned int max_x_grid, max_y_grid;
71  worldToGridBounded(*info_, max_x, max_y, max_x_grid, max_y_grid);
72  width_ = max_x_grid - min_x_ + 1;
73  height_ = max_y_grid - min_y_ + 1;
74 
75  // Initialize internal iterator
76  internal_iterator_.reset(new SubGrid(info_, min_x_, min_y_, width_, height_));
77  index_.x = min_x_;
78  index_.y = min_y_;
79 
80  // Iterate to first valid index
81  if (!isInside(index_.x, index_.y)) ++(*this);
84 }
85 
87  : PolygonFill(other.info_, other.index_, other.polygon_, other.min_x_, other.min_y_, other.width_, other.height_,
88  other.start_index_)
89 {
90 }
91 
93  nav_2d_msgs::Polygon2D polygon,
94  unsigned int min_x, unsigned int min_y, unsigned int width, unsigned int height,
95  const nav_grid::Index& start_index)
96  : BaseIterator(info, index), polygon_(polygon),
97  min_x_(min_x), min_y_(min_y), width_(width), height_(height), start_index_(start_index)
98 {
100 }
101 
103 {
104  info_ = other.info_;
105  index_ = other.index_;
106  polygon_ = other.polygon_;
107  min_x_ = other.min_x_;
108  min_y_ = other.min_y_;
109  width_ = other.width_;
110  height_ = other.height_;
111  start_index_ = other.start_index_;
113  return *this;
114 }
115 
116 bool PolygonFill::isInside(unsigned int x, unsigned int y) const
117 {
118  // Determine if the current index is inside the polygon using the number of crossings method
119  double wx, wy;
120  gridToWorld(*info_, x, y, wx, wy);
121  return nav_2d_utils::isInside(polygon_, wx, wy);
122 }
123 
125 {
127 }
128 
130 {
132  start_index_);
133 }
134 
136 {
137  ++(*internal_iterator_);
139  while (*internal_iterator_ != internal_iterator_->end())
140  {
141  if (isInside(index_.x, index_.y))
142  break;
143  ++(*internal_iterator_);
145  }
146 }
147 
149 {
150  return nav_2d_utils::equals(polygon_, other.polygon_);
151 }
152 
153 } // namespace nav_grid_iterators
PolygonFill(const nav_grid::NavGridInfo *info, nav_2d_msgs::Polygon2D polygon)
Public Constructor.
bool equals(const nav_2d_msgs::Polygon2D &polygon0, const nav_2d_msgs::Polygon2D &polygon1)
void gridToWorld(const NavGridInfo &info, int mx, int my, double &wx, double &wy)
nav_2d_msgs::Polygon2D polygon_
Definition: polygon_fill.h:102
void increment() override
Increase the iterator to the next element.
PolygonFill end() const override
Helper function for range-style iteration.
bool fieldsEqual(const PolygonFill &other) override
Additional check for whether fields of derived iterators are equal.
Iterates over all of the valid indexes that lie within an arbitrary polygon in row major order...
Definition: polygon_fill.h:49
PolygonFill begin() const override
Helper function for range-style iteration Equivalent to the above constructor.
Iterator for looping through every index within an aligned rectangular portion of the grid...
Definition: sub_grid.h:47
bool isInside(unsigned int x, unsigned int y) const
Check if given index is inside the polygon.
PolygonFill & operator=(const PolygonFill &other)
Assignment Operator Required to ensure unique_ptr is set properly.
bool worldToGridBounded(const NavGridInfo &info, double wx, double wy, unsigned int &mx, unsigned int &my)
bool isInside(const nav_2d_msgs::Polygon2D &polygon, const double x, const double y)
std::unique_ptr< SubGrid > internal_iterator_
Definition: polygon_fill.h:105


nav_grid_iterators
Author(s):
autogenerated on Sun Jan 10 2021 04:08:42