Use final path in entry point planner and output

diff --git a/src/test10.cc b/src/test10.cc
index fa87b18bd74865aed78914282c9d3c550d91c019..239c9ea0e89e7554385a8b35984beb173ce41264 100644 (file)
--- a/src/test10.cc
+++ b/src/test10.cc
@@ -30,6 +30,9 @@ enum EppStat {
 EppStat epp_stat = FINISHED;
 void epp_proc(
         Json::Value jvi
+        , std::vector<RRTNode> *fp
+        , double *fc
+        , double *oic
 )
 {
         epp_stat = RUNNING;
@@ -39,7 +42,16 @@ void epp_proc(
 
         while (epp.next()) {}
 
-        std::cout << epp.json() << std::endl;
+        if (epp.path().size() > 0) {
+                *fc = cc(*epp.path().back()) + *oic;
+                fp->clear();
+                for (auto n: epp.path()) {
+                        fp->push_back(RRTNode());
+                        fp->back().x(n->x());
+                        fp->back().y(n->y());
+                        fp->back().h(n->h());
+                }
+        }
         epp_stat = FINISHED;
 }
 
@@ -60,8 +72,36 @@ int main()
         orig_init.sp(2.7);
         //orig_init.st(M_PI / 32); // only for sc2_4
 
-        std::thread epp_thread(epp_proc, jvi);
+        std::thread epp_thread(
+                epp_proc
+                , jvi
+                , &final_path
+                , &final_path_cost
+                , &orig_init_cost
+        );
         epp_thread.join();
 
+        Json::Value jvo = jvi; // JSON output
+        {
+                jvo["path"][0][0] = orig_init.x();
+                jvo["path"][0][1] = orig_init.y();
+                jvo["path"][0][2] = orig_init.h();
+                unsigned int cu = 0;
+                unsigned int co = 0;
+                unsigned int pcnt = 1;
+                for (auto n: final_path) {
+                        jvo["path"][pcnt][0] = n.x();
+                        jvo["path"][pcnt][1] = n.y();
+                        jvo["path"][pcnt][2] = n.h();
+                        if (n.t(RRTNodeType::cusp))
+                                cu++;
+                        if (n.t(RRTNodeType::connected))
+                                co++;
+                        pcnt++;
+                }
+                jvo["cusps-in-path"] = cu;
+                jvo["connecteds-in-path"] = co;
+        }
+        std::cout << jvo << std::endl;
         return 0;
 }