namespace rrts {
-RRTExt13::DijkstraNode::DijkstraNode(RRTNode* n) : node(n)
-{
-}
-
-bool
-RRTExt13::DijkstraNode::vi()
-{
- if (this->v) {
- return true;
- }
- this->v = true;
- return false;
-}
-
-int
-RRTExt13::DijkstraNodeComparator::operator() (DijkstraNode const& n1,
- DijkstraNode const& n2)
-{
- return n1.node->cc() > n2.node->cc();
-}
-
-int
-RRTExt13::DijkstraNodeBackwardComparator::operator() (DijkstraNode const& n1,
- DijkstraNode const& n2)
-{
- return n1.d > n2.d;
-}
-
void
RRTExt13::interesting_forward()
{
i = j;
break;
}
- if (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp())) {
+ if (nj.p_is_cusp()) {
this->dn_.push_back(DijkstraNode(&njl));
this->dn_.back().i = this->dn_.size() - 1;
}
i = j;
break;
}
- if (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp())) {
+ if (nj.p_is_cusp()) {
this->dn_.push_back(DijkstraNode(&njl));
this->dn_.back().i = this->dn_.size() - 1;
this->dn_.back().d = goal_cc - njl.cc();
pq.push(this->dn_.front());
while (!pq.empty()) {
DijkstraNode fd = pq.top();
- RRTNode& f = *fd.node;
+ RRTNode& f = *fd.p_rrtnode;
pq.pop();
this->set_bc_pose_to(f);
for (unsigned int i = fd.i + 1; i < this->dn_.size(); i++) {
- RRTNode& t = *this->dn_[i].node;
+ RRTNode& t = *this->dn_[i].p_rrtnode;
if (!this->_bc.drivable(t)) {
continue;
}
|| ss != this->_steered.size()) {
continue;
}
+ // The first steered is the same as f.
+ this->_steered.erase(this->_steered.begin());
this->set_bc_pose_to(this->_steered.back());
bool td = this->_bc.drivable(t);
bool tn = this->_bc.edist(t) < 2.0 * this->_eta;
if (cost < t.cc() && td && tn) {
this->join_steered(&f);
- t.p(this->_nodes.back());
+ t.p(this->_nodes.back(), true);
t.c(this->cost_build(this->_nodes.back(), t));
if (!this->dn_[i].vi()) {
pq.push(this->dn_[i]);
pq.push(this->dn_.front());
while (!pq.empty()) {
DijkstraNode td = pq.top();
- RRTNode& t = *td.node;
+ RRTNode& t = *td.p_rrtnode;
pq.pop();
this->set_bc_pose_to(t);
for (unsigned int i = td.i; i > 0; i--) {
DijkstraNode& fd = this->dn_[i];
- RRTNode& f = *this->dn_[i].node;
+ RRTNode& f = *this->dn_[i].p_rrtnode;
if (!this->_bc.drivable(f)) {
continue;
}
|| ss != this->_steered.size()) {
continue;
}
+ // The first steered is the same as f.
+ this->_steered.erase(this->_steered.begin());
this->set_bc_pose_to(this->_steered.back());
bool td = this->_bc.drivable(t);
bool tn = this->_bc.edist(t) < 2.0 * this->_eta;
if (cost < fd.d && td && tn) {
fd.d = cost;
this->join_steered(&f);
- t.p(this->_nodes.back());
+ t.p(this->_nodes.back(), true);
t.c(this->cost_build(this->_nodes.back(), t));
this->recompute_cc_for_predecessors_and(&t);
if (!this->dn_[i].vi()) {