line.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 
39 
40 namespace nav_grid_iterators
41 {
42 Line::Line(const nav_grid::NavGridInfo* info, double x0, double y0, double x1, double y1,
43  bool include_last_index, bool bresenham)
44  : BaseIterator(info), x0_(x0), y0_(y0), x1_(x1), y1_(y1), include_last_index_(include_last_index),
45  bresenham_(bresenham), start_index_(0, 0), end_index_(0, 0)
46 {
48 
49  // Convenience variables to avoid mismatched comparisons
50  signed_width_ = static_cast<int>(info->width);
51  signed_height_ = static_cast<int>(info->height);
52 
53  // Cache the end index
54  nav_grid::SignedIndex end = internal_iterator_->getFinalIndex();
55  end_index_.x = end.x;
56  end_index_.y = end.y;
57 
58  // Iterate to first valid index
60  while (!internal_iterator_->isFinished() && !inBounds(sindex))
61  {
62  internal_iterator_->increment();
63  sindex = **internal_iterator_;
64  }
65 
66  // If all the indices are invalid, explicitly set the start index to be invalid
67  if (internal_iterator_->isFinished())
68  {
70  }
71  else
72  {
73  start_index_.x = sindex.x;
74  start_index_.y = sindex.y;
75  }
76 
78 }
79 
80 Line::Line(const Line& other)
81  : Line(other.info_, other.index_, other.x0_, other.y0_, other.x1_, other.y1_, other.include_last_index_,
82  other.bresenham_, other.start_index_, other.end_index_)
83 {
84 }
85 
86 Line::Line(const nav_grid::NavGridInfo* info, const nav_grid::Index& index, double x0, double y0, double x1, double y1,
87  bool include_last_index, bool bresenham, nav_grid::Index start_index, nav_grid::Index end_index)
88  : BaseIterator(info, index), x0_(x0), y0_(y0), x1_(x1), y1_(y1), include_last_index_(include_last_index),
89  bresenham_(bresenham), start_index_(start_index), end_index_(end_index)
90 {
92  signed_width_ = static_cast<int>(info->width);
93  signed_height_ = static_cast<int>(info->height);
94 }
95 
96 Line& Line::operator=(const Line& other)
97 {
98  info_ = other.info_;
99  index_ = other.index_;
100  x0_ = other.x0_;
101  y0_ = other.y0_;
102  x1_ = other.x1_;
103  y1_ = other.y1_;
105  bresenham_ = other.bresenham_;
106  start_index_ = other.start_index_;
107  end_index_ = other.end_index_;
111  return *this;
112 }
113 
115 {
116  // translate coordinates into grid coordinates
117  double local_x0, local_y0, local_x1, local_y1;
118  worldToGrid(*info_, x0_, y0_, local_x0, local_y0);
119  worldToGrid(*info_, x1_, y1_, local_x1, local_y1);
120 
121  if (bresenham_)
122  {
123  internal_iterator_.reset(new Bresenham(local_x0, local_y0, local_x1, local_y1, include_last_index_));
124  }
125  else
126  {
127  internal_iterator_.reset(new RayTrace(local_x0, local_y0, local_x1, local_y1, include_last_index_));
128  }
129 }
130 
132 {
133  return sindex.x >= 0 && sindex.y >= 0 && sindex.x < signed_width_ && sindex.y < signed_height_;
134 }
135 
137 {
139 }
140 
142 {
144 }
145 
147 {
148  internal_iterator_->increment();
150  if (!internal_iterator_->isFinished() && !inBounds(sindex))
151  {
152  index_ = end_index_;
153  }
154  else
155  {
156  index_.x = sindex.x;
157  index_.y = sindex.y;
158  }
159 }
160 
161 bool Line::fieldsEqual(const Line& other)
162 {
163  return x0_ == other.x0_ && y0_ == other.y0_ && x1_ == other.x1_ && y1_ == other.y1_ &&
165 }
166 
167 } // namespace nav_grid_iterators
nav_grid::Index start_index_
Definition: line.h:111
Line begin() const override
Helper function for range-style iteration Equivalent to the above constructor.
Definition: line.cpp:136
nav_grid::Index end_index_
Definition: line.h:111
std::unique_ptr< AbstractLineIterator > internal_iterator_
Definition: line.h:106
void increment() override
Increase the iterator to the next element.
Definition: line.cpp:146
Line end() const override
Helper function for range-style iteration.
Definition: line.cpp:141
const nav_grid::NavGridInfo * info_
Line Iterator using Bresenham&#39;s algorithm (no subpixel precision)
Definition: bresenham.h:46
Line Iterator with Ray Tracing (subpixel accuracy)
Definition: ray_trace.h:46
Iterates over all of the valid indexes of a line.
Definition: line.h:48
bool fieldsEqual(const Line &other) override
Additional check for whether fields of derived iterators are equal.
Definition: line.cpp:161
void worldToGrid(const NavGridInfo &info, double wx, double wy, double &mx, double &my)
Line & operator=(const Line &other)
Assignment Operator Required to ensure unique_ptr is set properly.
Definition: line.cpp:96
Line(const nav_grid::NavGridInfo *info, double x0, double y0, double x1, double y1, bool include_last_index=true, bool bresenham=true)
Public Constructor.
Definition: line.cpp:42
bool inBounds(const nav_grid::SignedIndex &sindex)
Check if a SignedIndex is within the bounds of the NavGrid.
Definition: line.cpp:131


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