ray_trace_iterator.h
Go to the documentation of this file.
00001 #ifndef OCCUPANCY_GRID_UTILS_RAY_TRACE_ITERATOR_H
00002 #define OCCUPANCY_GRID_UTILS_RAY_TRACE_ITERATOR_H
00003 
00004 #include <occupancy_grid_utils/coordinate_conversions.h>
00005 #include <ros/assert.h>
00006 #include <boost/iterator/iterator_facade.hpp>
00007 
00008 namespace occupancy_grid_utils
00009 {
00010 
00011 inline
00012 int8_t sign(const coord_t x)
00013 {
00014   return (x>0) ? 1 : -1;
00015 }
00016 
00017 class RayTraceIterator
00018   : public boost::iterator_facade<RayTraceIterator, Cell, 
00019                                   boost::forward_traversal_tag, const Cell&>
00020 {
00021 public:
00022 
00023   RayTraceIterator();
00024   RayTraceIterator (const Cell& c1, const Cell& c2, bool done=false) :
00025     cell_(c1), goal_(c2), done_(done)
00026   {
00027     const coord_t dx = c2.x-c1.x;
00028     const coord_t dy = c2.y-c1.y;
00029     const coord_t abs_dx = abs(dx);
00030     const coord_t abs_dy = abs(dy);
00031     const int8_t offset_dx = sign(dx);
00032     const int8_t offset_dy = sign(dy);
00033 
00034     if (abs_dx>abs_dy) {
00035       x_inc_ = offset_dx;
00036       y_inc_ = 0;
00037       x_correction_ = 0;
00038       y_correction_ = offset_dy;
00039       error_ = abs_dx/2;
00040       error_inc_ = abs_dy;
00041       error_threshold_ = abs_dx;
00042     }
00043     else {
00044       x_inc_ = 0;
00045       y_inc_ = offset_dy;
00046       x_correction_ = offset_dx;
00047       y_correction_ = 0;
00048       error_ = abs_dy/2;
00049       error_inc_ = abs_dx;
00050       error_threshold_ = abs_dy;
00051     }
00052 
00053     ROS_DEBUG_NAMED ("ray_trace_iter", "Setting up ray trace iterator from %d, %d to %d, %d", 
00054                      c1.x, c1.y, c2.x, c2.y);
00055     ROS_DEBUG_NAMED ("ray_trace_iter", " x_inc=%d, y_inc=%d, x_correction=%d, y_correction=%d",
00056                      x_inc_, y_inc_, x_correction_, y_correction_);
00057     ROS_DEBUG_NAMED ("ray_trace_iter", " error=%u, error_inc=%u, error_threshold=%u",
00058                      error_, error_inc_, error_threshold_);
00059   }
00060 
00061 private:
00062 
00063   /****************************************
00064    * Iterator facade ops
00065    ****************************************/
00066 
00067   friend class boost::iterator_core_access;
00068 
00069   void increment ()
00070   {
00071     ROS_ASSERT_MSG (!done_, "Can't increment ray trace iterator that's already reached the end");
00072     if (cell_ == goal_) {
00073       done_=true;
00074     }
00075     else {
00076       cell_.x += x_inc_;
00077       cell_.y += y_inc_;
00078       error_ += error_inc_;
00079       if (error_ >= error_threshold_) {
00080         cell_.x += x_correction_;
00081         cell_.y += y_correction_;
00082         error_ -= error_threshold_;
00083       }
00084       ROS_DEBUG_NAMED ("ray_trace_iter", "cell is %d, %d", cell_.x, cell_.y);
00085     }
00086   }
00087 
00088   const Cell& dereference () const
00089   {
00090     ROS_ASSERT_MSG (!done_, "Can't dereference ray trace iterator that has reached the end");
00091     return cell_;
00092   }
00093 
00094   bool equal (const RayTraceIterator& other) const
00095   {
00096     // incomplete, but this should only be called when other relevant state is equal
00097     return (this->done_ && other.done_) || (!this->done_ && !other.done_ && this->cell_ == other.cell_);
00098   }
00099 
00100 
00101   /****************************************
00102    * Internal state
00103    ****************************************/
00104 
00105   coord_t x_inc_, y_inc_, x_correction_, y_correction_;
00106   coord_t error_, error_inc_, error_threshold_;
00107   Cell cell_, goal_;
00108   bool done_;
00109 };
00110 
00111 
00112 
00113 
00114 
00115 } // namespace occupancy_grid_utils
00116 
00117 #endif // include guard


occupancy_grid_utils
Author(s): Bhaskara Marthi
autogenerated on Tue Jan 7 2014 11:17:09