Merge branch 'release/0.7.0'

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

diff --git a/incl/rrtnode.h b/incl/rrtnode.h
index ce830098227390bc788a782708bd7eec6450c7fa..a515386044d94aa3f320b145c8b0c5cca56c0893 100644 (file)
--- a/incl/rrtnode.h
+++ b/incl/rrtnode.h
@@ -20,31 +20,65 @@ 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 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 x_ = 0;
-                float y_ = 0;
-                float h_ = 0;
-
                 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_;
 
                 bool visited_ = false; // for DFS
+        protected:
+                float x_ = 0;
+                float y_ = 0;
+                float h_ = 0;
+                float t_ = 0;
+                float s_ = 0;
+
+                RRTNode *rs_ = nullptr; // random sample of added point
         public:
                 RRTNode();
                 RRTNode(float x, float y);
                 RRTNode(float x, float y, float h);
+                RRTNode(float x, float y, float h, float t);
+                RRTNode(float x, float y, float h, float t, float s);
 
                 // getter
                 float x() const;
                 float y() const;
                 float h() const;
+                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;
@@ -52,17 +86,34 @@ 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);
 
                 float ccost(float cost);
                 float dcost(float cost);
+                float ocost(float cost);
+                char tree(char t);
 
+                bool remove_parent();
                 bool parent(RRTNode *parent);
                 bool visit(bool v);
 
                 // 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 {
@@ -75,6 +126,8 @@ class RRTEdge {
 
                 RRTNode *init() const;
                 RRTNode *goal() const;
+
+                RRTNode *intersect(RRTEdge *e, bool segment);
 };
 
 #endif