From 284e28ec3499cb782b0099f1f98c4d4df1aee4f4 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 12 Jul 2021 13:20:11 +0200
Subject: [PATCH] Add reset method, remove debug print

---
 api/rrtce.h     | 40 ++++++++++++++++++++++++++++++++++++++++
 api/rrtext.h    |  3 +++
 src/rrtext13.cc | 19 ++++++++++---------
 src/rrtext3.cc  |  9 +++++++++
 src/rrtext8.cc  |  6 ++++++
 5 files changed, 68 insertions(+), 9 deletions(-)

diff --git a/api/rrtce.h b/api/rrtce.h
index 439b936..eadd1a8 100644
--- a/api/rrtce.h
+++ b/api/rrtce.h
@@ -29,6 +29,11 @@ class RRTCE35
                         RRTExt2::deinit();
                         RRTExt8::deinit();
                 }
+                void reset()
+                {
+                    RRTExt8::reset();
+                    RRTExt13::reset();
+                }
 };
 class RRTCE34
         : public RRTExt2
@@ -49,6 +54,11 @@ class RRTCE34
                         RRTExt2::deinit();
                         RRTExt8::deinit();
                 }
+                void reset()
+                {
+                    RRTExt8::reset();
+                    RRTExt3::reset();
+                }
 };
 class RRTCE33
         : public RRTExt2
@@ -68,6 +78,11 @@ class RRTCE33
                         RRTExt2::deinit();
                         RRTExt8::deinit();
                 }
+                void reset()
+                {
+                    RRTExt8::reset();
+                    RRTExt3::reset();
+                }
 };
 class RRTCE32
         : public RRTExt2
@@ -87,6 +102,11 @@ class RRTCE32
                         RRTExt2::deinit();
                         RRTExt8::deinit();
                 }
+                void reset()
+                {
+                    RRTExt8::reset();
+                    RRTExt3::reset();
+                }
 };
 class RRTCE31
         : public RRTExt2
@@ -106,6 +126,11 @@ class RRTCE31
                         RRTExt2::deinit();
                         RRTExt8::deinit();
                 }
+                void reset()
+                {
+                    RRTExt8::reset();
+                    RRTExt3::reset();
+                }
 };
 class RRTCE30
         : public RRTExt2
@@ -125,6 +150,11 @@ class RRTCE30
                         RRTExt2::deinit();
                         RRTExt8::deinit();
                 }
+                void reset()
+                {
+                    RRTExt8::reset();
+                    RRTExt3::reset();
+                }
 };
 
 class RRTCE29
@@ -418,6 +448,11 @@ class RRTCE14
                         RRTExt2::deinit();
                         RRTExt8::deinit();
                 }
+                void reset()
+                {
+                    RRTExt8::reset();
+                    RRTExt3::reset();
+                }
 };
 class RRTCE13
         : public RRTExt2
@@ -436,6 +471,11 @@ class RRTCE13
                         RRTExt2::deinit();
                         RRTExt8::deinit();
                 }
+                void reset()
+                {
+                    RRTExt8::reset();
+                    RRTExt3::reset();
+                }
 };
 class RRTCE12
         : public RRTExt2
diff --git a/api/rrtext.h b/api/rrtext.h
index 406afbb..4e1f825 100644
--- a/api/rrtext.h
+++ b/api/rrtext.h
@@ -20,6 +20,7 @@
 class RRTExt13 : public virtual RRTS {
         private:
         public:
+                void reset();
                 std::vector<RRTNode *> orig_path_;
                 double orig_path_cost_ = 9999;
                 std::vector<RRTNode *> first_optimized_path_;
@@ -173,6 +174,7 @@ class RRTExt8 : public virtual RRTS {
                     this->kdroot_ = nullptr;
                 }
                 void init();
+                void reset();
                 void deinit();
                 void store_node(RRTNode n);
                 RRTNode *nn(RRTNode &t);
@@ -287,6 +289,7 @@ class RRTExt4 : public virtual RRTS {
 class RRTExt3 : public virtual RRTS {
         private:
         public:
+                void reset();
                 std::vector<RRTNode *> orig_path_;
                 double orig_path_cost_ = 9999;
                 std::vector<RRTNode *> first_optimized_path_;
diff --git a/src/rrtext13.cc b/src/rrtext13.cc
index d0db3d1..563432e 100644
--- a/src/rrtext13.cc
+++ b/src/rrtext13.cc
@@ -1,6 +1,15 @@
 #include <queue>
 #include "rrtext.h"
-#include <iostream>
+
+void RRTExt13::reset()
+{
+        RRTS::reset();
+        this->orig_path().clear();
+        this->first_optimized_path().clear();
+        this->orig_path_cost_ = 9999.9;
+        this->first_optimized_path_cost_ = 9999.9;
+}
+
 void RRTExt13::first_path_optimization()
 {
         if (this->orig_path().size() == 0) {
@@ -67,10 +76,6 @@ void RRTExt13::first_path_optimization()
                         jbc.y(op[j]->y());
                         jbc.h(op[j]->h());
                         if (!jbc.drivable(ibc)) {
-                                //std::cerr<<*op[i];
-                                //std::cerr<<" to ";
-                                //std::cerr<<*op[j];
-                                //std::cerr<<std::endl;
                                 dn.push_back(DijkstraNode(*op[j-1]));
                                 dn.back().cc = op[j-1]->cc;
                                 dn.back().s = &dn.back();
@@ -194,10 +199,6 @@ void RRTExt13::second_path_optimization()
                         jbc.y(op[j]->y());
                         jbc.h(op[j]->h());
                         if (!jbc.drivable(ibc)) {
-                                //std::cerr<<*op[i];
-                                //std::cerr<<" to ";
-                                //std::cerr<<*op[j];
-                                //std::cerr<<std::endl;
                                 dn.push_back(DijkstraNode(*op[j-1]));
                                 dn.back().cc = op[j-1]->cc;
                                 dn.back().s = &dn.back();
diff --git a/src/rrtext3.cc b/src/rrtext3.cc
index 8077b7e..f2d342e 100644
--- a/src/rrtext3.cc
+++ b/src/rrtext3.cc
@@ -1,6 +1,15 @@
 #include <queue>
 #include "rrtext.h"
 
+void RRTExt3::reset()
+{
+        RRTS::reset();
+        this->orig_path().clear();
+        this->first_optimized_path().clear();
+        this->orig_path_cost_ = 9999.9;
+        this->first_optimized_path_cost_ = 9999.9;
+}
+
 void RRTExt3::first_path_optimization()
 {
         if (this->orig_path().size() == 0) {
diff --git a/src/rrtext8.cc b/src/rrtext8.cc
index fdeb5d3..8cd461e 100644
--- a/src/rrtext8.cc
+++ b/src/rrtext8.cc
@@ -114,6 +114,12 @@ void RRTExt8::init()
 {
 }
 
+void RRTExt8::reset()
+{
+        RRTS::reset();
+        this->delete_kd_nodes();
+}
+
 void RRTExt8::deinit()
 {
         this->delete_kd_nodes(this->kdroot_);
-- 
2.39.2