+/*
+ * 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 */