Update readme

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

diff --git a/src/rrtext11.cc b/src/rrtext11.cc
index e8be3500ee54eb79b4018f7b0293e6b7941175db..67e8496e46b09e97e8359637e934329d0fd1da28 100644 (file)
--- a/src/rrtext11.cc
+++ b/src/rrtext11.cc
@@ -1,27 +1,42 @@
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
 #include "rrtext.h"
 
 bool RRTExt11::goal_found(RRTNode &f)
 {
-        auto &g = this->goals().front();
-        double cost = this->cost_build(f, g);
-        auto fbc = BicycleCar();
-        fbc.x(f.x());
-        fbc.y(f.y());
-        fbc.h(f.h());
-        auto gbc = BicycleCar();
-        gbc.x(g.x());
-        gbc.y(g.y());
-        gbc.h(g.h());
-        if (gbc.drivable(fbc)) {
-                this->steer(f, g);
-                if (std::get<0>(this->collide_steered_from(f)))
-                        return false;
-                this->join_steered(&f);
-                if (g.p() == nullptr || cc(f) + cost < cc(g)) {
-                        g.p(&f);
-                        g.c(cost);
-                }
-                return true;
-        }
-        return false;
+       auto &g = this->goals().front();
+       auto fbc = BicycleCar();
+       fbc.x(f.x());
+       fbc.y(f.y());
+       fbc.h(f.h());
+       auto gbc = BicycleCar();
+       gbc.x(g.x());
+       gbc.y(g.y());
+       gbc.h(g.h());
+       bool drivable = false;
+       if (this->entry_set)
+           drivable = gbc.drivable(fbc, this->entry.b, this->entry.e);
+       else
+           drivable = gbc.drivable(fbc);
+       if (drivable) {
+               this->steer(f, g);
+               if (std::get<0>(this->collide_steered_from(f)))
+                       return false;
+               double cost = this->cost_build(f, g);
+               this->join_steered(&f);
+               if (g.p() == nullptr) {
+                       g.p(&f);
+                       g.c(cost);
+                       this->path_cost_before_opt_ = g.cc;
+               } else if (f.cc + cost < g.cc) {
+                       g.p(&f);
+                       g.c(cost);
+               }
+               return true;
+       }
+       return false;
 }