spiral.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 Spiral::Spiral(const nav_grid::NavGridInfo* info, double center_x, double center_y, double radius)
41  : BaseIterator(info), center_x_(center_x), center_y_(center_y), distance_(0), start_index_(0, 0)
42 {
43  radius_sq_ = radius * radius;
44  max_distance_ = ceil(radius / info->resolution);
45  loadRing();
48 }
49 
50 Spiral::Spiral(const Spiral& other)
51  : Spiral(other.info_, other.index_, other.center_x_, other.center_y_, other.radius_sq_,
52  other.distance_, other.max_distance_, other.start_index_)
53 {
54 }
55 
56 Spiral::Spiral(const nav_grid::NavGridInfo* info, const nav_grid::Index& index, double center_x, double center_y,
57  double radius_sq, unsigned int distance, unsigned int max_distance,
58  const nav_grid::Index& start_index)
59  : BaseIterator(info, index), center_x_(center_x), center_y_(center_y), radius_sq_(radius_sq),
60  distance_(distance), max_distance_(max_distance), start_index_(start_index)
61 {
62  loadRing();
64  {
67  }
68 }
69 
71 {
72  info_ = other.info_;
73  index_ = other.index_;
74  center_x_ = other.center_x_;
75  center_y_ = other.center_y_;
76  radius_sq_ = other.radius_sq_;
77  distance_ = other.distance_;
79  start_index_ = other.start_index_;
80  loadRing();
82  {
85  }
86  return *this;
87 }
88 
90 {
92 }
93 
95 {
97  start_index_);
98 }
99 
101 {
102  while (distance_ <= max_distance_)
103  {
104  ++(*internal_iterator_);
105  if (*internal_iterator_ == internal_iterator_->end())
106  {
107  ++distance_;
108  if (distance_ > max_distance_)
109  {
111  return;
112  }
113  loadRing();
114  }
116  if (isInside(index_.x, index_.y))
117  {
118  break;
119  }
120  }
121  if (distance_ > max_distance_)
122  {
124  }
125 }
126 
127 bool Spiral::fieldsEqual(const Spiral& other)
128 {
129  return center_x_ == other.center_x_ && center_y_ == other.center_y_ &&
130  radius_sq_ == other.radius_sq_ && distance_ == other.distance_;
131 }
132 
134 {
135  while (distance_ <= max_distance_)
136  {
138 
139  if (*internal_iterator_ != internal_iterator_->end())
140  break;
141  ++distance_;
142  }
143 }
144 
145 bool Spiral::isInside(unsigned int x, unsigned int y) const
146 {
147  double wx, wy;
148  gridToWorld(*info_, x, y, wx, wy);
149  double dx = wx - center_x_;
150  double dy = wy - center_y_;
151  return (dx * dx + dy * dy) < radius_sq_;
152 }
153 
154 } // namespace nav_grid_iterators
std::unique_ptr< CircleOutline > internal_iterator_
Definition: spiral.h:111
nav_grid::Index start_index_
Definition: spiral.h:110
void gridToWorld(const NavGridInfo &info, int mx, int my, double &wx, double &wy)
void increment() override
Increase the iterator to the next element.
Definition: spiral.cpp:100
unsigned int max_distance_
Definition: spiral.h:109
Iterates over all of the valid indexes that lie within a circle from the center out.
Definition: spiral.h:48
Spiral(const nav_grid::NavGridInfo *info, double center_x, double center_y, double radius)
Public Constructor.
Definition: spiral.cpp:40
bool fieldsEqual(const Spiral &other) override
Additional check for whether fields of derived iterators are equal.
Definition: spiral.cpp:127
bool isInside(unsigned int x, unsigned int y) const
Check if the center of the given index is within the circle.
Definition: spiral.cpp:145
unsigned int distance_
Definition: spiral.h:109
const nav_grid::NavGridInfo * info_
Spiral end() const override
Helper function for range-style iteration.
Definition: spiral.cpp:94
void loadRing()
Given a new distance value, loads the internal iterator. If there are no valid values in the internal...
Definition: spiral.cpp:133
Iterates over the valid indexes that lie on the outline of a circle.
Spiral & operator=(const Spiral &other)
Assignment Operator Required to ensure unique_ptr is set properly.
Definition: spiral.cpp:70
Spiral begin() const override
Helper function for range-style iteration Equivalent to the above constructor.
Definition: spiral.cpp:89


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