ray_trace.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 
36 #include <cmath>
37 #include <limits>
38 
39 namespace nav_grid_iterators
40 {
41 RayTrace::RayTrace(double x0, double y0, double x1, double y1, bool include_last_index)
42  : AbstractLineIterator(), x0_(x0), y0_(y0), x1_(x1), y1_(y1), include_last_index_(include_last_index)
43 {
44  dx_ = std::abs(x1 - x0);
45  dy_ = std::abs(y1 - y0);
46  index_.x = static_cast<int>(floor(x0));
47  index_.y = static_cast<int>(floor(y0));
48 
49  if (dx_ == 0)
50  {
51  loop_inc_x_ = 0;
52  error_ = std::numeric_limits<double>::max();
53  }
54  else if (x1 > x0)
55  {
56  loop_inc_x_ = 1;
57  error_ = (floor(x0) + 1 - x0) * dy_;
58  }
59  else
60  {
61  loop_inc_x_ = -1;
62  error_ = (x0 - floor(x0)) * dy_;
63  }
64 
65  if (dy_ == 0)
66  {
67  loop_inc_y_ = 0;
68  error_ -= std::numeric_limits<double>::max();
69  }
70  else if (y1 > y0)
71  {
72  loop_inc_y_ = 1;
73  error_ -= (floor(y0) + 1 - y0) * dx_;
74  }
75  else
76  {
77  loop_inc_y_ = -1;
78  error_ -= (y0 - floor(y0)) * dx_;
79  }
80 
81  /* Since we check if the index is equal to the second point in the line,
82  * we have to check for this one edge case to ensure we don't get into a rounding
83  * problem, resulting in an off-by-one error.
84  */
85  if (!include_last_index && x1 < x0 && y1 - floor(y1) == 0.0)
86  {
87  error_ += 1e-10;
88  }
89 
91 
92  // Special use case when start and end point are the same AND we want to include that point
93  if (include_last_index && loop_inc_x_ == 0 && loop_inc_y_ == 0)
94  {
95  loop_inc_x_ = 1;
96  }
97 }
98 
100  double x0, double y0, double x1, double y1, bool include_last_index,
101  double dx, double dy, double initial_error, int loop_inc_x, int loop_inc_y)
102  : AbstractLineIterator(index), x0_(x0), y0_(y0), x1_(x1), y1_(y1), include_last_index_(include_last_index),
103  dx_(dx), dy_(dy), error_(initial_error), initial_error_(initial_error),
104  loop_inc_x_(loop_inc_x), loop_inc_y_(loop_inc_y)
105 {
106 }
107 
109 {
112 }
113 
115 {
116  int x_diff = abs(static_cast<int>(x0_) - static_cast<int>(x1_));
117  int y_diff = abs(static_cast<int>(y0_) - static_cast<int>(y1_));
118  double final_error = initial_error_ - dx_ * y_diff + dy_ * x_diff;
120  dx_, dy_, final_error, loop_inc_x_, loop_inc_y_);
121 
122  // If we want the last_index, return an iterator that is whatever is one-past the end coordinates
124  end.increment();
125  return end;
126 }
127 
129 {
130  if (error_ > 0.0)
131  {
132  index_.y += loop_inc_y_;
133  error_ -= dx_;
134  }
135  else
136  {
137  index_.x += loop_inc_x_;
138  error_ += dy_;
139  }
140 }
141 
143 {
144  return end().index_;
145 }
146 } // namespace nav_grid_iterators
Abstract class for iterating over lines.
nav_grid::SignedIndex getFinalIndex() const override
Definition: ray_trace.cpp:142
Line Iterator with Ray Tracing (subpixel accuracy)
Definition: ray_trace.h:46
RayTrace begin() const
Helper function for range-style iteration.
Definition: ray_trace.cpp:108
RayTrace(double x0, double y0, double x1, double y1, bool include_last_index=true)
Public constructor.
Definition: ray_trace.cpp:41
void increment() override
Increase the iterator to the next element.
Definition: ray_trace.cpp:128
RayTrace end() const
Helper function for range-style iteration.
Definition: ray_trace.cpp:114


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