From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 10 Jun 2019 12:51:34 +0000 (+0200)
Subject: Update ccost of goal's parents
X-Git-Tag: v0.7.0~17^2~2
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/586b349a065ab5f6c1fd046a09293ee6c4120229

Update ccost of goal's parents
---

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 9f14eed..b21340f 100644
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -494,6 +494,19 @@ bool RRTBase::goal_found(
                                 return false;
                         }
                         this->goal_found_ = true;
+                        // Update ccost of goal's parents
+                        if (this->goals().size() > 0) {
+                                RRTNode *ch = this->goals().back();
+                                RRTNode *pn = this->goals().back()->parent();
+                                while (pn) {
+                                        pn->ccost(
+                                                ch->ccost()
+                                                - this->cost(pn, ch)
+                                        );
+                                        ch = pn;
+                                        pn = pn->parent();
+                                }
+                        }
                         return true;
                 }
         }