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         float h = this->h_;
  75         while (h > M_PI)
  76                 h -= 2 * M_PI;
  77         while (h <= -M_PI)
  78                 h += 2 * M_PI;
  79         return h;
  80 }
  81
  82 float RRTNode::t() const
  83 {
  84         return this->t_;
  85 }
  86
  87 float RRTNode::s() const
  88 {
  89         return this->s_;
  90 }
  91
  92 float RRTNode::ccost() const
  93 {
  94         return this->ccost_;
  95 }
  96
  97 float RRTNode::dcost() const
  98 {
  99         return this->dcost_;
 100 }
 101
 102 float RRTNode::ocost() const
 103 {
 104         return this->ocost_;
 105 }
 106
 107 char RRTNode::tree() const
 108 {
 109         return this->tree_;
 110 }
 111
 112 std::vector<RRTNode *> &RRTNode::children()
 113 {
 114         return this->children_;
 115 }
 116
 117 RRTNode *RRTNode::parent() const
 118 {
 119         return this->parent_;
 120 }
 121
 122 bool RRTNode::visited()
 123 {
 124         return this->visited_;
 125 }
 126
 127 // setter
 128 void RRTNode::h(float ch)
 129 {
 130         this->h_ = ch;
 131 }
 132
 133 void RRTNode::t(float ct)
 134 {
 135         this->t_ = ct;
 136 }
 137
 138 void RRTNode::s(float cs)
 139 {
 140         this->s_ = cs;
 141 }
 142
 143 bool RRTNode::add_child(RRTNode *node, float cost)
 144 {
 145         return this->add_child(node, cost, 1);
 146 }
 147
 148 bool RRTNode::add_child(RRTNode *node, float cost, float time)
 149 {
 150         node->dcost(cost);
 151         node->ccost(this->ccost() + cost);
 152         node->parent(this);
 153         node->t(this->t() + time);
 154         this->children_.push_back(node);
 155         return true;
 156 }
 157
 158 bool RRTNode::rem_child(RRTNode *node)
 159 {
 160         int i = 0;
 161         for (auto ch: this->children_) {
 162                 if (ch == node) {
 163                         this->children_.erase(this->children_.begin() + i);
 164                         return true;
 165                 }
 166                 i++;
 167         }
 168         return false;
 169 }
 170
 171 float RRTNode::ccost(float cost)
 172 {
 173         return this->ccost_ = cost;
 174 }
 175
 176 float RRTNode::dcost(float cost)
 177 {
 178         return this->dcost_ = cost;
 179 }
 180
 181 float RRTNode::ocost(float cost)
 182 {
 183         return this->ocost_ = cost;
 184 }
 185
 186 char RRTNode::tree(char t)
 187 {
 188         return this->tree_ = t;
 189 }
 190
 191 bool RRTNode::remove_parent()
 192 {
 193         this->parent_ = nullptr;
 194         return true;
 195 }
 196
 197 bool RRTNode::parent(RRTNode *parent)
 198 {
 199         if (parent == nullptr)
 200                 return false;
 201         this->parent_ = parent;
 202         return true;
 203 }
 204
 205 bool RRTNode::visit(bool v)
 206 {
 207         return this->visited_ = v;
 208 }
 209
 210 // other
 211 bool RRTNode::comp_ccost(RRTNode *n1, RRTNode *n2)
 212 {
 213         return n1->ccost() < n2->ccost();
 214 }
 215
 216 float RRTNode::update_ccost()
 217 {
 218         std::vector<RRTNode *> s; // DFS stack
 219         std::vector<RRTNode *> r; // reset visited_
 220         RRTNode *tmp;
 221         s.push_back(this);
 222         while (s.size() > 0) {
 223                 tmp = s.back();
 224                 s.pop_back();
 225                 if (!tmp->visit()) {
 226                         r.push_back(tmp);
 227                         for (auto ch: tmp->children()) {
 228                                 ch->ccost(tmp->ccost() + ch->dcost());
 229                                 s.push_back(ch);
 230                         }
 231                 }
 232         }
 233         for (auto n: r)
 234                 n->visit(false);
 235         return tmp->ccost();
 236 }
 237
 238 bool RRTNode::visit()
 239 {
 240         if (this->visited_) {
 241                 return true;
 242         }
 243         this->visited_ = true;
 244         return false;
 245 }
 246
 247 RRTEdge::RRTEdge():
 248         init_(new RRTNode()),
 249         goal_(new RRTNode())
 250 {}
 251
 252 RRTEdge::RRTEdge(RRTNode *init, RRTNode *goal):
 253         init_(init),
 254         goal_(goal)
 255 {}
 256
 257 RRTNode *RRTEdge::init() const
 258 {
 259         return this->init_;
 260 }
 261
 262 RRTNode *RRTEdge::goal() const
 263 {
 264         return this->goal_;
 265 }
 266
 267 RRTNode *RRTEdge::intersect(RRTEdge *e, bool segment)
 268 {
 269         // see https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
 270         float x1 = this->init()->x();
 271         float y1 = this->init()->y();
 272         float x2 = this->goal()->x();
 273         float y2 = this->goal()->y();
 274         float x3 = e->init()->x();
 275         float y3 = e->init()->y();
 276         float x4 = e->goal()->x();
 277         float y4 = e->goal()->y();
 278         float deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
 279         if (deno == 0)
 280                 return nullptr;
 281         if (!segment) {
 282                 float px = (x1 * y2 - y1 * x2) * (x3 - x4) -
 283                                 (x1 - x2) * (x3 * y4 - y3 * x4);
 284                 px /= deno;
 285                 float py = (x1 * y2 - y1 * x2) * (y3 - y4) -
 286                                 (y1 - y2) * (x3 * y4 - y3 * x4);
 287                 py /= deno;
 288                 return new RRTNode(px, py, 0);
 289         } else {
 290                 float t = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
 291                 t /= deno;
 292                 float u = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
 293                 u *= -1;
 294                 u /= deno;
 295                 if (t < 0 || t > 1 || u < 0 || u > 1)
 296                         return nullptr;
 297                 return new RRTNode(x1 + t * (x2 - x1), y1 + t * (y2 - y1), 0);
 298         }
 299         return nullptr;
 300 }