}
void
-RRTNode::p(RRTNode& p, bool connecting_goal)
+RRTNode::p(RRTNode& p, bool can_be_too_close)
{
assert(this != &p);
- if (!connecting_goal) {
+ if (!can_be_too_close) {
assert(!(std::abs(p.x() - this->x()) < 1e-3
&& std::abs(p.y() - this->y()) < 1e-3
&& std::abs(p.h() - this->h()) < 1e-3));
void
RRTS::recompute_cc_for_predecessors_and(RRTNode* g)
{
- assert(this->_path.size() == 0);
+ std::vector<RRTNode*> path;
+ path.reserve(this->_path.size());
while (g != nullptr) {
- this->_path.push_back(g);
+ path.push_back(g);
g = g->p();
}
- std::reverse(this->_path.begin(), this->_path.end());
- for (unsigned int i = 1; i < this->_path.size(); i++) {
- this->_path[i]->c(this->cost_build(
- *this->_path[i - 1],
- *this->_path[i]));
+ std::reverse(path.begin(), path.end());
+ for (unsigned int i = 1; i < path.size(); i++) {
+ path[i]->c(this->cost_build(*path[i - 1], *path[i]));
}
- this->_path.clear();
}
void
jvo["paths"][j][i][2] = n.h();
jvo["paths"][j][i][3] = n.sp();
jvo["paths"][j][i][4] = n.st();
- jvo["paths"][j][i][5] = n.p_is_cusp();
+ jvo["paths"][j][i][5] = false;
+ if (n.p_is_cusp()) {
+ assert(i > 0);
+ jvo["paths"][j][i - 1][5] = true;
+ }
i++;
}
+ // Initial point is part of the first segment.
+ jvo["paths"][j][0][3] = jvo["paths"][j][1][3];
+ jvo["paths"][j][0][4] = jvo["paths"][j][1][4];
+ // Goal point is part of the last segment.
+ jvo["paths"][j][i - 1][3] = jvo["paths"][j][i - 2][3];
+ jvo["paths"][j][i - 1][4] = jvo["paths"][j][i - 2][4];
+ // --
jvo["costs"][j] = path.back().cc();
j++;
}
jvo["path"][i][2] = n->h();
jvo["path"][i][3] = n->sp();
jvo["path"][i][4] = n->st();
- jvo["path"][i][5] = n->p_is_cusp();
+ jvo["path"][i][5] = false;
+ if (n->p_is_cusp()) {
+ assert(i > 0);
+ jvo["path"][i - 1][5] = true;
+ }
i++;
}
+ // Initial point is part of the first segment.
+ jvo["path"][0][3] = jvo["path"][1][3];
+ jvo["path"][0][4] = jvo["path"][1][4];
+ // Goal point is part of the last segment.
+ jvo["path"][i - 1][3] = jvo["path"][i - 2][3];
+ jvo["path"][i - 1][4] = jvo["path"][i - 2][4];
+ // --
jvo["costs"][j] = this->_path.back()->cc();
- j++;
jvo["goal_cc"] = this->_goal.cc(); // TODO remove, use the following
jvo["cost"] = this->path_cost();
jvo["time"] = this->scnt();