circle_outline.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 CircleOutline::CircleOutline(const nav_grid::NavGridInfo* info, double center_x, double center_y, double radius)
41  : CircleOutline(info, center_x, center_y, static_cast<unsigned int>(ceil(radius / info->resolution)))
42 {
43 }
44 
45 CircleOutline::CircleOutline(const nav_grid::NavGridInfo* info, double center_x, double center_y, unsigned int radius)
46  : BaseIterator(info), distance_(radius), init_(false)
47 {
48  signed_width_ = static_cast<int>(info->width);
49  signed_height_ = static_cast<int>(info->height);
50 
51  // Calculate and save the center coordinates
52  worldToGrid(*info_, center_x, center_y, center_index_x_, center_index_y_);
53 
55  point_y_ = 0;
56 
58  {
59  increment();
61  }
65 }
66 
68  int center_index_x, int center_index_y, unsigned int distance,
69  bool init, const nav_grid::Index& start_index)
70  : BaseIterator(info, index),
71  center_index_x_(center_index_x), center_index_y_(center_index_y), distance_(distance), init_(init),
72  start_index_(start_index)
73 {
74  signed_width_ = static_cast<int>(info->width);
75  signed_height_ = static_cast<int>(info->height);
77  point_y_ = 0;
78 }
79 
81 {
83  distance_, false, start_index_);
84 }
85 
87 {
89  distance_, true, start_index_);
90 }
91 
93 {
94  init_ = true;
95  while (true)
96  {
97  int nx = -signum(point_y_);
98  int ny = signum(point_x_);
99  if (nx != 0 && isOnOutline(point_x_ + nx, point_y_))
100  {
101  point_x_ += nx;
102  }
103  else if (ny != 0 && isOnOutline(point_x_, point_y_ + ny))
104  {
105  point_y_ += ny;
106  }
107  else
108  {
109  point_x_ += nx;
110  point_y_ += ny;
111  }
112 
114  {
115  break;
116  }
117  if (point_x_ == static_cast<int>(distance_) && point_y_ == 0)
118  {
120  return;
121  }
122  }
125 }
126 
128 {
129  return center_index_x_ == other.center_index_x_ && center_index_y_ == other.center_index_y_ &&
130  distance_ == other.distance_ && init_ == other.init_;
131 }
132 
133 bool CircleOutline::isValidIndex(int x, int y) const
134 {
135  return x >= 0 && y >= 0 && x < signed_width_ && y < signed_height_;
136 }
137 
138 bool CircleOutline::isOnOutline(int dx, int dy) const
139 {
140  return static_cast<unsigned int>(hypot(dx, dy)) == distance_;
141 }
142 
143 } // namespace nav_grid_iterators
bool isOnOutline(int dx, int dy) const
Check if a cell with the given distance from the center of the circle is on the outline of the circle...
bool fieldsEqual(const CircleOutline &other) override
Additional check for whether fields of derived iterators are equal.
bool isValidIndex(int x, int y) const
Check if arbitrary coordinates are within the grid.
CircleOutline end() const override
Helper function for range-style iteration.
Iterates over the valid indexes that lie on the outline of a circle.
int signum(const int val)
returns the sign of a number
CircleOutline begin() const override
Helper function for range-style iteration Equivalent to the above constructor.
void worldToGrid(const NavGridInfo &info, double wx, double wy, double &mx, double &my)
void increment() override
Increase the iterator to the next element.
CircleOutline(const nav_grid::NavGridInfo *info, double center_x, double center_y, double radius)
Public Constructor.


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