Add commented steer instead of overlap in T3

diff --git a/base/main.cc b/base/main.cc
index d715b2e12c2669f17e390c20b852310025eff4c0..a2cc7a2632c49d1f096612ce3edd96faade74fc7 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -265,6 +265,7 @@ int main()
         int tol = 0;
         int ndl = 0;
         bool ndone = true;
+        std::vector<RRTNode *> steered;
         while (!gf && p.elapsed() < TMAX &&
                         p.p_root_.nodes().size() < NOFNODES &&
                         p.p_goal_.nodes().size() < NOFNODES) {
@@ -285,6 +286,35 @@ for (auto gn: p.p_goal_.ixy_[i][j].nodes()) {
                 gon = gn;
                 mc = rn->ccost() + gn->ccost();
         }
+        //// TRY TO STEER
+        //steered.clear();
+        //steered = p.steer(rn, gn);
+        //for (unsigned int k = 0; k < steered.size() - 1; k++)
+        //        steered[k]->add_child(steered[k + 1], 1);
+        //if (p.collide(
+        //                steered[0],
+        //                steered[steered.size() - 1])) {
+        //        for (auto n: steered)
+        //                delete n;
+        //        continue;
+        //}
+        //if (steered[steered.size() - 1]->ccost() +
+        //                rn->ccost() +
+        //                gn->ccost() < mc) {
+        //        gf = true;
+        //        p.goal_found(true);
+        //        ron = rn;
+        //        gon = gn;
+        //        mc = steered[steered.size() - 1]->ccost() +
+        //                rn->ccost() +
+        //                gn->ccost();
+        //}
+        //for (auto n: steered)
+        //        delete n;
+        //p.tend();
+        //if (p.elapsed() >= TMAX)
+        //        goto escapeloop;
+        //// END OF STEER
 }}
                         }
                         tol++;
@@ -306,6 +336,17 @@ escapeloop:
         //std::cerr << "rgf is " << p.p_root_.goal_found() << std::endl;
         //std::cerr << "ggf is " << p.p_goal_.goal_found() << std::endl;
         //std::cerr << "cgf is " << p.goal_found() << std::endl;
+        //if (p.goal_found()) {
+        //        steered.clear();
+        //        steered = p.steer(ron, gon);
+        //        for (auto ns: steered) {
+        //                p.nodes().push_back(ns);
+        //                p.add_ixy(ns);
+        //                ron->add_child(ns, p.cost(ron, ns));
+        //                ron = ns;
+        //        }
+        //        mc = ron->ccost() + gon->ccost();
+        //}
         if (p.p_root_.goal_found() && p.p_root_.goal()->ccost() < mc) {
                 ron = p.p_root_.goal()->parent();
                 gon = p.p_root_.goal();