Add rrtext3 to compound extensions 18 - 33

[hubacji1/rrts.git] / src / rrtext3.cc

diff --git a/src/rrtext3.cc b/src/rrtext3.cc
index f8c694c3f71fb52c9348106e4bd97569f89a4481..426137207104a4ba13fb7f967139069851fb1fc7 100644 (file)
--- a/src/rrtext3.cc
+++ b/src/rrtext3.cc
@@ -105,3 +105,32 @@ std::vector<RRTNode *> RRTExt3::path()
         }
         return RRTS::path();
 }
+
+Json::Value RRTExt3::json()
+{
+        Json::Value jvo = RRTS::json();
+        jvo["orig_path_cost"] = this->orig_path_cost();
+        {
+                unsigned int cu = 0;
+                unsigned int co = 0;
+                unsigned int pcnt = 0;
+                for (auto n: this->path()) {
+                        jvo["orig_path"][pcnt][0] = n->x();
+                        jvo["orig_path"][pcnt][1] = n->y();
+                        jvo["orig_path"][pcnt][2] = n->h();
+                        if (n->t(RRTNodeType::cusp))
+                                cu++;
+                        if (n->t(RRTNodeType::connected))
+                                co++;
+                        pcnt++;
+                }
+                jvo["orig_cusps-in-path"] = cu;
+                jvo["orig_connecteds-in-path"] = co;
+        }
+        return jvo;
+}
+
+void RRTExt3::json(Json::Value jvi)
+{
+        return RRTS::json(jvi);
+}