incl/rrtnode.h

   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 #ifndef RRTNODE_H
  19 #define RRTNODE_H
  20
  21 #include <vector>
  22
  23 class RRTNode {
  24         private:
  25                 float dcost_ = 0; // direct cost (of edge) to parent
  26                 float ccost_ = 0; // cumulative cost from root
  27                 float ocost_ = 0; // distance to the nearest obstacle
  28
  29                 RRTNode *parent_ = nullptr;
  30                 std::vector<RRTNode *> children_;
  31
  32                 bool visited_ = false; // for DFS
  33         protected:
  34                 float x_ = 0;
  35                 float y_ = 0;
  36                 float h_ = 0;
  37                 float t_ = 0;
  38         public:
  39                 RRTNode();
  40                 RRTNode(float x, float y);
  41                 RRTNode(float x, float y, float h);
  42                 RRTNode(float x, float y, float h, float t);
  43
  44                 // getter
  45                 float x() const;
  46                 float y() const;
  47                 float h() const;
  48                 float t() const;
  49
  50                 float ccost() const;
  51                 float dcost() const;
  52                 float ocost() const;
  53
  54                 std::vector<RRTNode *> &children();
  55                 RRTNode *parent() const;
  56
  57                 bool visited();
  58
  59                 // setter
  60                 void t(float ct);
  61                 bool add_child(RRTNode *node, float cost);
  62                 bool add_child(RRTNode *node, float cost, float time);
  63                 bool rem_child(RRTNode *node);
  64
  65                 float ccost(float cost);
  66                 float dcost(float cost);
  67                 float ocost(float cost);
  68
  69                 bool remove_parent();
  70                 bool parent(RRTNode *parent);
  71                 bool visit(bool v);
  72
  73                 // other
  74                 static bool comp_ccost(RRTNode *n1, RRTNode *n2);
  75                 bool visit();
  76 };
  77
  78 class RRTEdge {
  79         private:
  80                 RRTNode *init_;
  81                 RRTNode *goal_;
  82         public:
  83                 RRTEdge();
  84                 RRTEdge(RRTNode *init, RRTNode *goal);
  85
  86                 RRTNode *init() const;
  87                 RRTNode *goal() const;
  88 };
  89
  90 #endif