]> rtime.felk.cvut.cz Git - hubacji1/iamcar.git/blobdiff - incl/rrtnode.h
Merge branch 'release/0.7.0'
[hubacji1/iamcar.git] / incl / rrtnode.h
index 1ab68932c69c6504708fa456face7f5983b8d9ff..a515386044d94aa3f320b145c8b0c5cca56c0893 100644 (file)
@@ -20,11 +20,21 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 
 #include <vector>
 
-#define IS_NEAR(a, b) ({ __typeof__ (a) _a = (a); \
-                __typeof__ (b) _b = (b); \
-                pow(pow(_b->x() - _a->x(), 2) + \
-                                pow(_b->y() - _a->y(), 2), 0.5) < 0.2 && \
-                std::abs(_b->h() - _a->h()) < M_PI / 32 ? true : false; })
+#define IS_NEAR(a, b) ({ \
+        __typeof__ (a) _a = (a); \
+        __typeof__ (b) _b = (b); \
+        pow(pow(_b->x() - _a->x(), 2) \
+        + pow(_b->y() - _a->y(), 2), 0.5) < 0.2 \
+        && std::abs(_b->h() - _a->h()) < M_PI / 32 ? true : false; \
+})
+
+#define GOAL_IS_NEAR(a, b) ({ \
+        __typeof__ (a) _a = (a); \
+        __typeof__ (b) _b = (b); \
+        pow(pow(_b->x() - _a->x(), 2) \
+        + pow(_b->y() - _a->y(), 2), 0.5) < 0.05 \
+        && std::abs(_b->h() - _a->h()) < M_PI / 32 ? true : false; \
+})
 
 template <typename T> int sgn(T val) {
             return (T(0) < val) - (val < T(0));
@@ -35,6 +45,7 @@ class RRTNode {
                 float dcost_ = 0; // direct cost (of edge) to parent
                 float ccost_ = 0; // cumulative cost from root
                 float ocost_ = 0; // distance to the nearest obstacle
+                char tree_ = '0'; // tree affinity
 
                 RRTNode *parent_ = nullptr;
                 std::vector<RRTNode *> children_;
@@ -46,6 +57,8 @@ class RRTNode {
                 float h_ = 0;
                 float t_ = 0;
                 float s_ = 0;
+
+                RRTNode *rs_ = nullptr; // random sample of added point
         public:
                 RRTNode();
                 RRTNode(float x, float y);
@@ -60,9 +73,12 @@ class RRTNode {
                 float t() const;
                 float s() const;
 
+                RRTNode *rs() const;
+
                 float ccost() const;
                 float dcost() const;
                 float ocost() const;
+                char tree() const;
 
                 std::vector<RRTNode *> &children();
                 RRTNode *parent() const;
@@ -73,6 +89,9 @@ class RRTNode {
                 void h(float ch);
                 void t(float ct);
                 void s(float cs);
+
+                void rs(RRTNode *rs);
+
                 bool add_child(RRTNode *node, float cost);
                 bool add_child(RRTNode *node, float cost, float time);
                 bool rem_child(RRTNode *node);
@@ -80,6 +99,7 @@ class RRTNode {
                 float ccost(float cost);
                 float dcost(float cost);
                 float ocost(float cost);
+                char tree(char t);
 
                 bool remove_parent();
                 bool parent(RRTNode *parent);
@@ -89,6 +109,11 @@ class RRTNode {
                 static bool comp_ccost(RRTNode *n1, RRTNode *n2);
                 float update_ccost();
                 bool visit();
+                /** Return ``true`` if ``n`` is in front of ``this``.
+                *
+                * @param n The node that is beeing checked.
+                */
+                bool inFront(RRTNode *n);
 };
 
 class RRTEdge {
@@ -101,6 +126,8 @@ class RRTEdge {
 
                 RRTNode *init() const;
                 RRTNode *goal() const;
+
+                RRTNode *intersect(RRTEdge *e, bool segment);
 };
 
 #endif