Merge branch 'release/0.7.0'

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

diff --git a/incl/rrtnode.h b/incl/rrtnode.h
index 9926b77753280f9d44a1b8d783887b2892583f91..a515386044d94aa3f320b145c8b0c5cca56c0893 100644 (file)
--- a/incl/rrtnode.h
+++ b/incl/rrtnode.h
@@ -20,10 +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 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_;
@@ -34,20 +56,29 @@ class RRTNode {
                 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;
@@ -55,13 +86,20 @@ 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);
@@ -69,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 {
@@ -82,6 +126,8 @@ class RRTEdge {
 
                 RRTNode *init() const;
                 RRTNode *goal() const;
+
+                RRTNode *intersect(RRTEdge *e, bool segment);
 };
 
 #endif