}
if (ps.cusp().size() > 0) {
p.goal(ps.getMidd());
- p.goals(ps.goals());
p.slot_cusp(ps.cusp().front()); // use first found solution
+ p.goals(ps.goals());
jvo["midd"][0] = p.goal()->x();
jvo["midd"][1] = p.goal()->y();
jvo["midd"][2] = p.goal()->h();
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;
+ }
+ }
+ return false;
+}
+
bool RRTBase::collide(RRTNode *init, RRTNode *goal)
{
std::vector<RRTEdge *> edges;
this->add_ixy(ns);
this->ocost(ns);
pn = ns;
- RRTNode *oldgoal = this->goal();
- this->goal(go);
- if (this->goal_found(pn, CO)) {
- this->goal_cost();
+ if (this->goal_found(pn, go)) {
this->tlog(this->findt());
en_add = false;
- } else {
- this->goal(oldgoal);
}
}
}
bool goal_found(
RRTNode *node,
float (*cost)(RRTNode *, RRTNode *));
+ bool goal_found(
+ RRTNode *node,
+ RRTNode *goal
+ );
bool collide(RRTNode *init, RRTNode *goal);
bool optp_dijkstra(
std::vector<RRTNode *> &cusps,