base/rrtnode.cc

   1 /*
   2 This file is part of I am car.
   3
   4 I am car is free software: you can redistribute it and/or modify
   5 it under the terms of the GNU General Public License as published by
   6 the Free Software Foundation, either version 3 of the License, or
   7 (at your option) any later version.
   8
   9 I am car is distributed in the hope that it will be useful,
  10 but WITHOUT ANY WARRANTY; without even the implied warranty of
  11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12 GNU General Public License for more details.
  13
  14 You should have received a copy of the GNU General Public License
  15 along with I am car. If not, see <http://www.gnu.org/licenses/>.
  16 */
  17
  18 #include <cmath>
  19 #include "rrtnode.h"
  20
  21 RRTNode::RRTNode():
  22         x_(0),
  23         y_(0),
  24         h_(0),
  25         t_(0),
  26         s_(0)
  27 {}
  28
  29 RRTNode::RRTNode(float x, float y):
  30         x_(x),
  31         y_(y),
  32         h_(0),
  33         t_(0),
  34         s_(0)
  35 {}
  36
  37 RRTNode::RRTNode(float x, float y, float h):
  38         x_(x),
  39         y_(y),
  40         h_((h > 2 * M_PI) ? h - 2 * M_PI : h),
  41         t_(0),
  42         s_(0)
  43 {}
  44
  45 RRTNode::RRTNode(float x, float y, float h, float t):
  46         x_(x),
  47         y_(y),
  48         h_((h > 2 * M_PI) ? h - 2 * M_PI : h),
  49         t_(t),
  50         s_(0)
  51 {}
  52
  53 RRTNode::RRTNode(float x, float y, float h, float t, float s):
  54         x_(x),
  55         y_(y),
  56         h_((h > 2 * M_PI) ? h - 2 * M_PI : h),
  57         t_(t),
  58         s_(s)
  59 {}
  60
  61 // getter
  62 float RRTNode::x() const
  63 {
  64         return this->x_;
  65 }
  66
  67 float RRTNode::y() const
  68 {
  69         return this->y_;
  70 }
  71
  72 float RRTNode::h() const
  73 {
  74         return this->h_;
  75 }
  76
  77 float RRTNode::t() const
  78 {
  79         return this->t_;
  80 }
  81
  82 float RRTNode::s() const
  83 {
  84         return this->s_;
  85 }
  86
  87 float RRTNode::ccost() const
  88 {
  89         return this->ccost_;
  90 }
  91
  92 float RRTNode::dcost() const
  93 {
  94         return this->dcost_;
  95 }
  96
  97 float RRTNode::ocost() const
  98 {
  99         return this->ocost_;
 100 }
 101
 102 char RRTNode::tree() const
 103 {
 104         return this->tree_;
 105 }
 106
 107 std::vector<RRTNode *> &RRTNode::children()
 108 {
 109         return this->children_;
 110 }
 111
 112 RRTNode *RRTNode::parent() const
 113 {
 114         return this->parent_;
 115 }
 116
 117 bool RRTNode::visited()
 118 {
 119         return this->visited_;
 120 }
 121
 122 // setter
 123 void RRTNode::h(float ch)
 124 {
 125         this->h_ = ch;
 126 }
 127
 128 void RRTNode::t(float ct)
 129 {
 130         this->t_ = ct;
 131 }
 132
 133 void RRTNode::s(float cs)
 134 {
 135         this->s_ = cs;
 136 }
 137
 138 bool RRTNode::add_child(RRTNode *node, float cost)
 139 {
 140         return this->add_child(node, cost, 1);
 141 }
 142
 143 bool RRTNode::add_child(RRTNode *node, float cost, float time)
 144 {
 145         node->dcost(cost);
 146         node->ccost(this->ccost() + cost);
 147         node->parent(this);
 148         node->t(this->t() + time);
 149         this->children_.push_back(node);
 150         return true;
 151 }
 152
 153 bool RRTNode::rem_child(RRTNode *node)
 154 {
 155         int i = 0;
 156         for (auto ch: this->children_) {
 157                 if (ch == node) {
 158                         this->children_.erase(this->children_.begin() + i);
 159                         return true;
 160                 }
 161                 i++;
 162         }
 163         return false;
 164 }
 165
 166 float RRTNode::ccost(float cost)
 167 {
 168         return this->ccost_ = cost;
 169 }
 170
 171 float RRTNode::dcost(float cost)
 172 {
 173         return this->dcost_ = cost;
 174 }
 175
 176 float RRTNode::ocost(float cost)
 177 {
 178         return this->ocost_ = cost;
 179 }
 180
 181 char RRTNode::tree(char t)
 182 {
 183         return this->tree_ = t;
 184 }
 185
 186 bool RRTNode::remove_parent()
 187 {
 188         this->parent_ = nullptr;
 189         return true;
 190 }
 191
 192 bool RRTNode::parent(RRTNode *parent)
 193 {
 194         if (parent == nullptr)
 195                 return false;
 196         this->parent_ = parent;
 197         return true;
 198 }
 199
 200 bool RRTNode::visit(bool v)
 201 {
 202         return this->visited_ = v;
 203 }
 204
 205 // other
 206 bool RRTNode::comp_ccost(RRTNode *n1, RRTNode *n2)
 207 {
 208         return n1->ccost() < n2->ccost();
 209 }
 210
 211 float RRTNode::update_ccost()
 212 {
 213         std::vector<RRTNode *> s; // DFS stack
 214         std::vector<RRTNode *> r; // reset visited_
 215         RRTNode *tmp;
 216         s.push_back(this);
 217         while (s.size() > 0) {
 218                 tmp = s.back();
 219                 s.pop_back();
 220                 if (!tmp->visit()) {
 221                         r.push_back(tmp);
 222                         for (auto ch: tmp->children()) {
 223                                 ch->ccost(tmp->ccost() + ch->dcost());
 224                                 s.push_back(ch);
 225                         }
 226                 }
 227         }
 228         for (auto n: r)
 229                 n->visit(false);
 230         return tmp->ccost();
 231 }
 232
 233 bool RRTNode::visit()
 234 {
 235         if (this->visited_) {
 236                 return true;
 237         }
 238         this->visited_ = true;
 239         return false;
 240 }
 241
 242 RRTEdge::RRTEdge():
 243         init_(new RRTNode()),
 244         goal_(new RRTNode())
 245 {}
 246
 247 RRTEdge::RRTEdge(RRTNode *init, RRTNode *goal):
 248         init_(init),
 249         goal_(goal)
 250 {}
 251
 252 RRTNode *RRTEdge::init() const
 253 {
 254         return this->init_;
 255 }
 256
 257 RRTNode *RRTEdge::goal() const
 258 {
 259         return this->goal_;
 260 }
 261
 262 RRTNode *RRTEdge::intersect(RRTEdge *e, bool segment)
 263 {
 264         // see https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
 265         float x1 = this->init()->x();
 266         float y1 = this->init()->y();
 267         float x2 = this->goal()->x();
 268         float y2 = this->goal()->y();
 269         float x3 = e->init()->x();
 270         float y3 = e->init()->y();
 271         float x4 = e->goal()->x();
 272         float y4 = e->goal()->y();
 273         float deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
 274         if (deno == 0)
 275                 return nullptr;
 276         if (!segment) {
 277                 float px = (x1 * y2 - y1 * x2) * (x3 - x4) -
 278                                 (x1 - x2) * (x3 * y4 - y3 * x4);
 279                 px /= deno;
 280                 float py = (x1 * y2 - y1 * x2) * (y3 - y4) -
 281                                 (y1 - y2) * (x3 * y4 - y3 * x4);
 282                 py /= deno;
 283                 return new RRTNode(px, py, 0);
 284         } else {
 285                 float t = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
 286                 t /= deno;
 287                 float u = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
 288                 u *= -1;
 289                 u /= deno;
 290                 if (t < 0 || t > 1 || u < 0 || u > 1)
 291                         return nullptr;
 292                 return new RRTNode(x1 + t * (x2 - x1), y1 + t * (y2 - y1), 0);
 293         }
 294         return nullptr;
 295 }