Add simple occupancy grid collision check

[hubacji1/rrts.git] / src / rrtext21.cc

diff --git a/src/rrtext21.cc b/src/rrtext21.cc
new file mode 100644 (file)
index 0000000..df7cd94
--- /dev/null
+++ b/src/rrtext21.cc
@@ -0,0 +1,55 @@
+/*
+ * SPDX-FileCopyrightText: 2022 Jiri Vlasak
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include <cassert>
+#include <cmath>
+#include "rrtext.hh"
+
+namespace rrts {
+
+bool
+RRTExt21::collide(RRTNode const &n)
+{
+       int col = floor((n.x() - this->_origin_x) / this->_grid_res);
+       int row = floor((n.y() - this->_origin_y) / this->_grid_res);
+       if (col < 0 || row < 0 || col >= this->_grid_width
+                       || row >= this->_grid_height) {
+               // grid is too small or sampling too far
+               return true;
+       }
+       if (this->_grid_data[row * this->_grid_width + col] > 65) {
+               return true;
+       }
+       return false;
+}
+
+bool
+RRTExt21::collide_steered()
+{
+       unsigned int i = 0;
+       for (auto &n: this->steered_) {
+               if (this->collide(n)) {
+                       break;
+               }
+               i++;
+       }
+       this->steered_.erase(this->steered_.begin() + i, this->steered_.end());
+       return this->steered_.size() == 0;
+}
+
+void
+RRTExt21::set_grid_to_check(unsigned int w, unsigned int h, float r,
+               int8_t const *d, double x, double y)
+{
+       this->_grid_width = w;
+       this->_grid_height = h;
+       this->_grid_res = r;
+       this->_grid_data = d;
+       this->_origin_x = x;
+       this->_origin_y = y;
+}
+
+} /* namespace rrts */