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 "rrtnode.h"
  19 RRTNode::RRTNode():
  20         x_(0),
  21         y_(0),
  22         h_(0)
  23 {}
  24
  25 RRTNode::RRTNode(float x, float y):
  26         x_(x),
  27         y_(y),
  28         h_(0)
  29 {}
  30
  31 RRTNode::RRTNode(float x, float y, float h):
  32         x_(x),
  33         y_(y),
  34         h_(h)
  35 {}
  36
  37 // getter
  38 float RRTNode::x() const
  39 {
  40         return this->x_;
  41 }
  42
  43 float RRTNode::y() const
  44 {
  45         return this->y_;
  46 }
  47
  48 float RRTNode::h() const
  49 {
  50         return this->h_;
  51 }
  52
  53 float RRTNode::ccost() const
  54 {
  55         return this->ccost_;
  56 }
  57
  58 float RRTNode::dcost() const
  59 {
  60         return this->dcost_;
  61 }
  62
  63 std::vector<RRTNode *> &RRTNode::children()
  64 {
  65         return this->children_;
  66 }
  67
  68 RRTNode *RRTNode::parent() const
  69 {
  70         return this->parent_;
  71 }
  72
  73 bool RRTNode::visited()
  74 {
  75         return this->visited_;
  76 }
  77
  78 // setter
  79 bool RRTNode::add_child(RRTNode *node, float cost)
  80 {
  81         node->dcost(cost);
  82         node->ccost(this->ccost() + cost);
  83         node->parent(this);
  84         this->children_.push_back(node);
  85         return true;
  86 }
  87
  88 float RRTNode::ccost(float cost)
  89 {
  90         return this->ccost_ = cost;
  91 }
  92
  93 float RRTNode::dcost(float cost)
  94 {
  95         return this->dcost_ = cost;
  96 }
  97
  98 bool RRTNode::parent(RRTNode *parent)
  99 {
 100         if (parent == nullptr)
 101                 return false;
 102         this->parent_ = parent;
 103         return true;
 104 }
 105
 106 bool RRTNode::visit(bool v)
 107 {
 108         return this->visited_ = v;
 109 }
 110
 111 // other
 112 bool RRTNode::comp_ccost(RRTNode *n1, RRTNode *n2)
 113 {
 114         return n1->ccost() < n2->ccost();
 115 }
 116
 117 bool RRTNode::visit()
 118 {
 119         if (this->visited_) {
 120                 return true;
 121         }
 122         this->visited_ = true;
 123         return false;
 124 }