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