circle_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 
38 namespace nav_grid_iterators
39 {
40 CircleFill::CircleFill(const nav_grid::NavGridInfo* info, double center_x, double center_y, double radius)
41  : BaseIterator(info), center_x_(center_x), center_y_(center_y), start_index_(0, 0)
42 {
43  radius_sq_ = radius * radius;
44 
45  double min_x = center_x_ - radius;
46  double max_x = center_x_ + radius;
47  double min_y = center_y_ - radius;
48  double max_y = center_y_ + radius;
49 
50  // Calculate and save the minimum coordinates
51  worldToGridBounded(*info_, min_x, min_y, min_x_, min_y_);
52 
53  // Calculate the max coordinates, and save the width/height
54  unsigned int max_x_grid, max_y_grid;
55  worldToGridBounded(*info_, max_x, max_y, max_x_grid, max_y_grid);
56 
57  width_ = max_x_grid - min_x_ + 1;
58  height_ = max_y_grid - min_y_ + 1;
59 
60  // Initialize internal iterator
61  internal_iterator_.reset(new SubGrid(info_, min_x_, min_y_, width_, height_));
62  index_.x = min_x_;
63  index_.y = min_y_;
64 
65  // Iterate to first valid index
66  if (!isInside(min_x_, min_y_)) ++(*this);
69 }
70 
72  : CircleFill(other.info_, other.index_, other.center_x_, other.center_y_, other.radius_sq_,
73  other.min_x_, other.min_y_, other.width_, other.height_, other.start_index_)
74 {
75 }
76 
77 CircleFill::CircleFill(const nav_grid::NavGridInfo* info, const nav_grid::Index& index, double center_x,
78  double center_y, double radius_sq, unsigned int min_x, unsigned int min_y, unsigned int width,
79  unsigned int height, const nav_grid::Index& start_index)
80  : BaseIterator(info, index), center_x_(center_x), center_y_(center_y), radius_sq_(radius_sq),
81  min_x_(min_x), min_y_(min_y), width_(width), height_(height), start_index_(start_index)
82 {
84 }
85 
87 {
88  info_ = other.info_;
89  index_ = other.index_;
90  center_x_ = other.center_x_;
91  center_y_ = other.center_y_;
92  radius_sq_ = other.radius_sq_;
93  min_x_ = other.min_x_;
94  min_y_ = other.min_y_;
95  width_ = other.width_;
96  height_ = other.height_;
97  start_index_ = other.start_index_;
99  return *this;
100 }
101 
102 bool CircleFill::isInside(unsigned int x, unsigned int y) const
103 {
104  double wx, wy;
105  gridToWorld(*info_, x, y, wx, wy);
106  double dx = wx - center_x_;
107  double dy = wy - center_y_;
108  return (dx * dx + dy * dy) < radius_sq_;
109 }
110 
112 {
114  start_index_);
115 }
116 
118 {
121 }
122 
124 {
125  ++(*internal_iterator_);
127  while (*internal_iterator_ != internal_iterator_->end())
128  {
129  if (isInside(index_.x, index_.y))
130  break;
131  ++(*internal_iterator_);
133  }
134 }
135 
137 {
138  return center_x_ == other.center_x_ && center_y_ == other.center_y_ && radius_sq_ == other.radius_sq_;
139 }
140 
141 } // namespace nav_grid_iterators
CircleFill & operator=(const CircleFill &other)
Assignment Operator Required to ensure unique_ptr is set properly.
Definition: circle_fill.cpp:86
CircleFill(const nav_grid::NavGridInfo *info, double center_x, double center_y, double radius)
Public Constructor.
Definition: circle_fill.cpp:40
void gridToWorld(const NavGridInfo &info, int mx, int my, double &wx, double &wy)
Iterates over all of the valid indexes that lie within a circle in row major order.
Definition: circle_fill.h:48
std::unique_ptr< SubGrid > internal_iterator_
Definition: circle_fill.h:108
bool isInside(unsigned int x, unsigned int y) const
Check if coordinates are inside the circle.
void increment() override
Increase the iterator to the next element.
Iterator for looping through every index within an aligned rectangular portion of the grid...
Definition: sub_grid.h:47
bool fieldsEqual(const CircleFill &other) override
Additional check for whether fields of derived iterators are equal.
CircleFill begin() const override
Helper function for range-style iteration Equivalent to the above constructor.
bool worldToGridBounded(const NavGridInfo &info, double wx, double wy, unsigned int &mx, unsigned int &my)
CircleFill end() const override
Helper function for range-style iteration.


nav_grid_iterators
Author(s):
autogenerated on Wed Jun 26 2019 20:06:20