From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 3 May 2021 12:46:05 +0000 (+0200)
Subject: Store entries
X-Git-Tag: v0.7.0~17
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/rrts.git/commitdiff_plain/9b27bccf8020585dcbee44a63891b3e1daa7ceb3

Store entries
---

diff --git a/api/rrts.h b/api/rrts.h
index e5821a0..2ce7e94 100644
--- a/api/rrts.h
+++ b/api/rrts.h
@@ -217,6 +217,9 @@ class RRTS {
                 /// ---
                 struct { double x=0; double y=0; double b=0; double e=0; } entry;
                 bool entry_set = false;
+                struct { double x=0; double y=0; double h=0; } entry1;
+                struct { double x=0; double y=0; double h=0; } entry2;
+                bool entries_set = false;
                 std::vector<RRTNode *> steered1_;
                 std::vector<RRTNode *> steered2_;
                 /// ---
diff --git a/src/rrts.cc b/src/rrts.cc
index 217e44a..7a8083d 100644
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -617,6 +617,18 @@ Json::Value RRTS::json()
                         jvo["entry"][0] = this->goals().front().x();
                         jvo["entry"][1] = this->goals().front().y();
                         jvo["entry"][2] = this->goals().front().h();
+                        if (this->entry_set) {
+                            jvo["entry"][2] = this->entry.b;
+                            jvo["entry"][3] = this->entry.e;
+                        }
+                        if (this->entries_set) {
+                                jvo["entries"][0][0] = this->entry1.x;
+                                jvo["entries"][0][1] = this->entry1.y;
+                                jvo["entries"][0][2] = this->entry1.h;
+                                jvo["entries"][1][0] = this->entry2.x;
+                                jvo["entries"][1][1] = this->entry2.y;
+                                jvo["entries"][1][2] = this->entry2.h;
+                        }
                         jvo["goal"][0] = this->goals().back().x();
                         jvo["goal"][1] = this->goals().back().y();
                         jvo["goal"][2] = this->goals().back().h();
@@ -721,6 +733,15 @@ void RRTS::json(Json::Value jvi)
                         this->goals().back().p(gp);
                         gp = &this->goals().back();
                 }
+                if (jvi["entries"] != Json::nullValue) {
+                        this->entries_set = true;
+                        this->entry1.x = jvi["entries"][0][0].asDouble();
+                        this->entry1.y = jvi["entries"][0][1].asDouble();
+                        this->entry1.h = jvi["entries"][0][2].asDouble();
+                        this->entry2.x = jvi["entries"][1][0].asDouble();
+                        this->entry2.y = jvi["entries"][1][1].asDouble();
+                        this->entry2.h = jvi["entries"][1][2].asDouble();
+                }
                 for (auto g: jvi["goals"]) {
                         RRTNode tmp_node;
                         tmp_node.x(g[0].asDouble());