Merge branch 'release/0.7.0'

[hubacji1/iamcar.git] / incl / rrtnode.h

diff --git a/incl/rrtnode.h b/incl/rrtnode.h
index 5bc5f810a7a170d13ee046f66698b78695809410..a515386044d94aa3f320b145c8b0c5cca56c0893 100644 (file)
--- a/incl/rrtnode.h
+++ b/incl/rrtnode.h
@@ -20,17 +20,32 @@ 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));
+}
 
 class RRTNode {
         private:
                 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_;
@@ -42,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);
@@ -56,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;
@@ -66,8 +86,12 @@ class RRTNode {
                 bool visited();
 
                 // setter
+                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);
@@ -75,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);
@@ -82,7 +107,13 @@ class RRTNode {
 
                 // other
                 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 {
@@ -95,6 +126,8 @@ class RRTEdge {
 
                 RRTNode *init() const;
                 RRTNode *goal() const;
+
+                RRTNode *intersect(RRTEdge *e, bool segment);
 };
 
 #endif