Merge branch 'fix-steer'

[hubacji1/iamcar2.git] / rrts / src / rrtext13.cc

diff --git a/rrts/src/rrtext13.cc b/rrts/src/rrtext13.cc
index 758f4d994b42b12941ec11e8ad779654c0387302..1619d7bce3e583bffdcbb6e96cd89e30b1cbec41 100644 (file)
--- a/rrts/src/rrtext13.cc
+++ b/rrts/src/rrtext13.cc
@@ -10,34 +10,6 @@
 
 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()
 {
@@ -129,11 +101,11 @@ RRTExt13::dijkstra_forward()
        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;
                        }
@@ -171,12 +143,12 @@ RRTExt13::dijkstra_backward()
        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;
                        }
@@ -209,7 +181,7 @@ RRTExt13::dijkstra_backward()
 }
 
 void
-RRTExt13::compute_path()
+RRTExt13::compute_path(void)
 {
        RRTS::compute_path();
        if (this->_goal.cc() == 0.0 || this->_path.size() == 0) {
@@ -255,7 +227,7 @@ RRTExt13::RRTExt13()
 }
 
 Json::Value
-RRTExt13::json() const
+RRTExt13::json(void) const
 {
        auto jvo = RRTS::json();
        unsigned int i = 0;
@@ -272,7 +244,7 @@ RRTExt13::json() const
 }
 
 void
-RRTExt13::reset()
+RRTExt13::reset(void)
 {
        RRTS::reset();
        this->opath_.clear();