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