From 37aff9d7009d36ebedc5e552f935597fdc7c10e9 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 3 Dec 2018 13:45:44 +0100
Subject: [PATCH] Add nodes tree affinity

This allows developing of advanced RRT algorithms as RRT*-Connect.
---
 base/rrtnode.cc | 10 ++++++++++
 incl/rrtnode.h  |  3 +++
 2 files changed, 13 insertions(+)

diff --git a/base/rrtnode.cc b/base/rrtnode.cc
index f721cc8..f77e543 100644
--- a/base/rrtnode.cc
+++ b/base/rrtnode.cc
@@ -99,6 +99,11 @@ float RRTNode::ocost() const
         return this->ocost_;
 }
 
+char RRTNode::tree() const
+{
+        return this->tree_;
+}
+
 std::vector<RRTNode *> &RRTNode::children()
 {
         return this->children_;
@@ -173,6 +178,11 @@ float RRTNode::ocost(float cost)
         return this->ocost_ = cost;
 }
 
+char RRTNode::tree(char t)
+{
+        return this->tree_ = t;
+}
+
 bool RRTNode::remove_parent()
 {
         this->parent_ = nullptr;
diff --git a/incl/rrtnode.h b/incl/rrtnode.h
index 1ab6893..6acec4a 100644
--- a/incl/rrtnode.h
+++ b/incl/rrtnode.h
@@ -35,6 +35,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_;
@@ -63,6 +64,7 @@ class RRTNode {
                 float ccost() const;
                 float dcost() const;
                 float ocost() const;
+                char tree() const;
 
                 std::vector<RRTNode *> &children();
                 RRTNode *parent() const;
@@ -80,6 +82,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);
-- 
2.39.2