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 template <typename T> int sgn(T val) {
  30             return (T(0) < val) - (val < T(0));
  31 }
  32
  33 class RRTNode {
  34         private:
  35                 float dcost_ = 0; // direct cost (of edge) to parent
  36                 float ccost_ = 0; // cumulative cost from root
  37                 float ocost_ = 0; // distance to the nearest obstacle
  38                 char tree_ = '0'; // tree affinity
  39
  40                 RRTNode *parent_ = nullptr;
  41                 std::vector<RRTNode *> children_;
  42
  43                 bool visited_ = false; // for DFS
  44         protected:
  45                 float x_ = 0;
  46                 float y_ = 0;
  47                 float h_ = 0;
  48                 float t_ = 0;
  49                 float s_ = 0;
  50
  51                 RRTNode *rs_ = nullptr; // random sample of added point
  52         public:
  53                 RRTNode();
  54                 RRTNode(float x, float y);
  55                 RRTNode(float x, float y, float h);
  56                 RRTNode(float x, float y, float h, float t);
  57                 RRTNode(float x, float y, float h, float t, float s);
  58
  59                 // getter
  60                 float x() const;
  61                 float y() const;
  62                 float h() const;
  63                 float t() const;
  64                 float s() const;
  65
  66                 RRTNode *rs() const;
  67
  68                 float ccost() const;
  69                 float dcost() const;
  70                 float ocost() const;
  71                 char tree() const;
  72
  73                 std::vector<RRTNode *> &children();
  74                 RRTNode *parent() const;
  75
  76                 bool visited();
  77
  78                 // setter
  79                 void h(float ch);
  80                 void t(float ct);
  81                 void s(float cs);
  82
  83                 void rs(RRTNode *rs);
  84
  85                 bool add_child(RRTNode *node, float cost);
  86                 bool add_child(RRTNode *node, float cost, float time);
  87                 bool rem_child(RRTNode *node);
  88
  89                 float ccost(float cost);
  90                 float dcost(float cost);
  91                 float ocost(float cost);
  92                 char tree(char t);
  93
  94                 bool remove_parent();
  95                 bool parent(RRTNode *parent);
  96                 bool visit(bool v);
  97
  98                 // other
  99                 static bool comp_ccost(RRTNode *n1, RRTNode *n2);
 100                 float update_ccost();
 101                 bool visit();
 102 };
 103
 104 class RRTEdge {
 105         private:
 106                 RRTNode *init_;
 107                 RRTNode *goal_;
 108         public:
 109                 RRTEdge();
 110                 RRTEdge(RRTNode *init, RRTNode *goal);
 111
 112                 RRTNode *init() const;
 113                 RRTNode *goal() const;
 114
 115                 RRTNode *intersect(RRTEdge *e, bool segment);
 116 };
 117
 118 #endif