src/rrtext20.cc

   1 /*
   2  * SPDX-FileCopyrightText: 2022 Jiri Vlasak
   3  *
   4  * SPDX-License-Identifier: GPL-3.0-only
   5  */
   6
   7 #include <cassert>
   8 #include "rrtext.hh"
   9
  10 namespace rrts {
  11
  12 bool
  13 RRTExt20::collide_steered()
  14 {
  15         assert(this->_points_to_check != nullptr);
  16         std::vector<bcar::Point> points_to_check(*this->_points_to_check);
  17         unsigned int i = 0;
  18         for (auto &n: this->steered_) {
  19                 for (auto &p: points_to_check) {
  20                         Point const n_p(n);
  21                         CarSize const n_cs(this->bc());
  22                         if (p.inside_of(n_p, n_cs.edist_to_rr())) {
  23                                 break;
  24                         }
  25                         if (p.edist(n_p) < n_cs.edist_to_lf()
  26                                         && p.edist(n_p) > n_cs.edist_to_rr()) {
  27                                 BicycleCar n_bc(this->bc());
  28                                 n_bc.x(n.x());
  29                                 n_bc.y(n.y());
  30                                 n_bc.h(n.h());
  31                                 std::vector<Point> poly;
  32                                 poly.push_back(n_bc.lf());
  33                                 poly.push_back(n_bc.lr());
  34                                 poly.push_back(n_bc.rr());
  35                                 poly.push_back(n_bc.rf());
  36                                 poly.push_back(n_bc.lf());
  37                                 if (p.inside_of(poly)) {
  38                                         break;
  39                                 }
  40
  41                         }
  42                 }
  43                 i++;
  44         }
  45         this->steered_.erase(this->steered_.begin() + i, this->steered_.end());
  46         return this->steered_.size() == 0;
  47 }
  48
  49 void
  50 RRTExt20::set_points_to_check(std::vector<bcar::Point> const *p)
  51 {
  52         this->_points_to_check = p;
  53 }
  54
  55 } /* namespace rrts */