src/rrtext21.cc

   1 /*
   2  * SPDX-FileCopyrightText: 2022 Jiri Vlasak
   3  *
   4  * SPDX-License-Identifier: GPL-3.0-only
   5  */
   6
   7 #include <cassert>
   8 #include <cmath>
   9 #include "rrtext.hh"
  10
  11 namespace rrts {
  12
  13 bool
  14 RRTExt21::collide(RRTNode const &n)
  15 {
  16         int col = floor((n.x() - this->_origin_x) / this->_grid_res);
  17         int row = floor((n.y() - this->_origin_y) / this->_grid_res);
  18         if (col < 0 || row < 0 || col >= this->_grid_width
  19                         || row >= this->_grid_height) {
  20                 // grid is too small or sampling too far
  21                 return true;
  22         }
  23         if (this->_grid_data[row * this->_grid_width + col] > 65) {
  24                 return true;
  25         }
  26         return false;
  27 }
  28
  29 bool
  30 RRTExt21::collide_steered()
  31 {
  32         unsigned int i = 0;
  33         for (auto &n: this->steered_) {
  34                 if (this->collide(n)) {
  35                         break;
  36                 }
  37                 i++;
  38         }
  39         this->steered_.erase(this->steered_.begin() + i, this->steered_.end());
  40         return this->steered_.size() == 0;
  41 }
  42
  43 void
  44 RRTExt21::set_grid_to_check(unsigned int w, unsigned int h, float r,
  45                 int8_t const *d, double x, double y)
  46 {
  47         this->_grid_width = w;
  48         this->_grid_height = h;
  49         this->_grid_res = r;
  50         this->_grid_data = d;
  51         this->_origin_x = x;
  52         this->_origin_y = y;
  53 }
  54
  55 } /* namespace rrts */