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