rrts/src/rrtext2.cc

   1 /*
   2  * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
   3  *
   4  * SPDX-License-Identifier: GPL-3.0-only
   5  */
   6
   7 #include <cassert>
   8 #include "rrtext.hh"
   9 #define CUTE_C2_IMPLEMENTATION
  10 #include "cute_c2.h"
  11
  12 namespace rrts {
  13
  14 bool
  15 RRTExt2::collide(RRTNode const& n)
  16 {
  17         this->c2x_bc_.p.x = n.x();
  18         this->c2x_bc_.p.y = n.y();
  19         this->c2x_bc_.r.c = cos(n.h());
  20         this->c2x_bc_.r.s = sin(n.h());
  21         for (auto& o: this->c2_obstacles_) {
  22                 if (c2PolytoPoly(&this->c2_bc_, &this->c2x_bc_, &o, NULL)) {
  23                         return true;
  24                 }
  25         }
  26         return false;
  27 }
  28
  29 bool
  30 RRTExt2::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         // The first node of this->_steered is the same as nn. Therefore, if
  41         // there is only one node left, it is nn, and other nodes collide.
  42         return this->_steered.size() == 1;
  43 }
  44
  45 RRTExt2::RRTExt2() : RRTS()
  46 {
  47         this->c2_bc_.count = 4;
  48         this->c2_bc_.verts[0].x = this->_bc.lfx();
  49         this->c2_bc_.verts[0].y = this->_bc.lfy();
  50         this->c2_bc_.verts[1].x = this->_bc.lrx();
  51         this->c2_bc_.verts[1].y = this->_bc.lry();
  52         this->c2_bc_.verts[2].x = this->_bc.rrx();
  53         this->c2_bc_.verts[2].y = this->_bc.rry();
  54         this->c2_bc_.verts[3].x = this->_bc.rfx();
  55         this->c2_bc_.verts[3].y = this->_bc.rfy();
  56 }
  57
  58 void
  59 RRTExt2::reset()
  60 {
  61         RRTS::reset();
  62         this->c2_bc_.count = 4;
  63         this->c2_bc_.verts[0].x = this->_bc.lfx();
  64         this->c2_bc_.verts[0].y = this->_bc.lfy();
  65         this->c2_bc_.verts[1].x = this->_bc.lrx();
  66         this->c2_bc_.verts[1].y = this->_bc.lry();
  67         this->c2_bc_.verts[2].x = this->_bc.rrx();
  68         this->c2_bc_.verts[2].y = this->_bc.rry();
  69         this->c2_bc_.verts[3].x = this->_bc.rfx();
  70         this->c2_bc_.verts[3].y = this->_bc.rfy();
  71 }
  72
  73 void
  74 RRTExt2::json(Json::Value jvi)
  75 {
  76         RRTS::json(jvi);
  77         assert(jvi["obst"] != Json::nullValue);
  78         for (auto& o: jvi["obst"]) {
  79                 assert(o.size() < C2_MAX_POLYGON_VERTS);
  80                 c2Poly c2tmp;
  81                 c2tmp.count = o.size();
  82                 unsigned int i = 0;
  83                 for (auto& c: o) {
  84                         c2tmp.verts[i].x = c[0].asDouble();
  85                         c2tmp.verts[i].y = c[1].asDouble();
  86                         i++;
  87                 }
  88                 this->c2_obstacles_.push_back(c2tmp);
  89         }
  90 }
  91
  92 } // namespace rrts