return this->goal_;
}
+std::vector<RRTNode *> &RRTBase::goals()
+{
+ return this->goals_;
+}
+
std::vector<RRTNode *> &RRTBase::nodes()
{
return this->nodes_;
this->goal_ = node;
}
+void RRTBase::goals(std::vector<RRTNode *> g)
+{
+ this->goals_ = g;
+ std::reverse(this->goals_.begin(), this->goals_.end());
+ RRTNode *pn = this->goals_.front();
+ for (auto n: this->goals_) {
+ if (n != pn) {
+ pn->add_child(n, this->cost(pn ,n));
+ pn = n;
+ }
+ }
+}
+
bool RRTBase::logr(RRTNode *root)
{
std::vector<RRTEdge *> e; // Edges to log
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;
+ }
+ }
+ return false;
+}
+
+bool RRTBase::goal_found(
+ RRTNode *node,
+ RRTNode *goal
+)
+{
+ if (IS_NEAR(node, goal)) {
+ if (this->goal_found_) {
+ if (
+ goal->ccost() != -1
+ && node->ccost() + this->cost(node, goal)
+ < goal->ccost()
+ ) {
+ RRTNode *op; // old parent
+ float oc; // old cumulative cost
+ float od; // old direct cost
+ op = goal->parent();
+ oc = goal->ccost();
+ od = goal->dcost();
+ node->add_child(goal,
+ this->cost(node, goal));
+ if (this->collide(node, goal)) {
+ node->children().pop_back();
+ goal->parent(op);
+ goal->ccost(oc);
+ goal->dcost(od);
+ } else {
+ op->rem_child(goal);
+ return true;
+ }
+ } else {
+ return false;
+ }
+ } else {
+ node->add_child(
+ goal,
+ this->cost(node, goal)
+ );
+ if (this->collide(node, goal)) {
+ node->children().pop_back();
+ goal->remove_parent();
+ return false;
+ }
+ this->goal_found_ = true;
+ // Update ccost of goal's children
+ goal->update_ccost();
+ // Update ccost of goals
+ for (auto g: this->goals()) {
+ if (g == goal)
+ break;
+ g->ccost(-1);
+ }
return true;
}
}