Add random sample getter/setter to RRTNode

diff --git a/base/rrtnode.cc b/base/rrtnode.cc
index 8127f3d4cfd9d9194985f2b3e366fa1873544ab7..59d5d4a07852ae225c5e2babb976b41efa135a4a 100644 (file)
--- a/base/rrtnode.cc
+++ b/base/rrtnode.cc
@@ -89,6 +89,11 @@ float RRTNode::s() const
         return this->s_;
 }
 
+RRTNode *RRTNode::rs() const
+{
+        return this->rs_;
+}
+
 float RRTNode::ccost() const
 {
         return this->ccost_;
@@ -140,6 +145,11 @@ void RRTNode::s(float cs)
         this->s_ = cs;
 }
 
+void RRTNode::rs(RRTNode *rs)
+{
+        this->rs_ = rs;
+}
+
 bool RRTNode::add_child(RRTNode *node, float cost)
 {
         return this->add_child(node, cost, 1);

diff --git a/incl/rrtnode.h b/incl/rrtnode.h
index fcb9d344049acbb5a94fa8dbde61e14988ce45ea..4ea4adfcfb57032d2a0852d29b50c8fb7247735f 100644 (file)
--- a/incl/rrtnode.h
+++ b/incl/rrtnode.h
@@ -47,6 +47,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);
@@ -61,6 +63,8 @@ class RRTNode {
                 float t() const;
                 float s() const;
 
+                RRTNode *rs() const;
+
                 float ccost() const;
                 float dcost() const;
                 float ocost() const;
@@ -75,6 +79,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);