Add initialization for connect trees procedure

[hubacji1/iamcar.git] / base / main.cc

diff --git a/base/main.cc b/base/main.cc
index 3098a69f748f47bd58e72743886d29f0c31b2726..87e0c154f017c7f7480dc05c4c859d097963bb4a 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -228,6 +228,16 @@ int main()
         }
         pthread_join(rt, NULL);
         pthread_join(gt, NULL);
+        if (p.p_root_.goal_found() && p.p_root_.goal()->ccost() < mc) {
+                ron = p.p_root_.goal()->parent();
+                gon = p.p_root_.goal();
+                mc = p.p_root_.goal()->ccost();
+        }
+        if (p.p_goal_.goal_found() && p.p_goal_.goal()->ccost() < mc) {
+                ron = p.p_goal_.goal();
+                gon = p.p_goal_.goal()->parent();
+                mc = p.p_goal_.goal()->ccost();
+        }
         // connect trees
         while (gon != p.goal()) {
                 p.p_root_.nodes().push_back(new RRTNode(
@@ -244,6 +254,7 @@ int main()
         }
         ron->add_child(p.goal(), p.p_root_.cost(ron, p.goal()));
         // end of connect trees
+        p.root()->remove_parent();  // needed if p.p_goal_.goal_found()
         if (gf)
                 p.tlog(p.findt());
         if (p.opt_path())