Merge branch 'fix-steer'

[hubacji1/iamcar2.git] / rrts / incl / rrtext.hh

diff --git a/rrts/incl/rrtext.hh b/rrts/incl/rrtext.hh
index 2fa6958b264639735958569ae2cc82c18764c949..24a206f331ba3d9406f08a1b479e9d57cf8920a7 100644 (file)
--- a/rrts/incl/rrtext.hh
+++ b/rrts/incl/rrtext.hh
@@ -123,20 +123,33 @@ class RRTExt13 : public virtual RRTS {
 private:
        class DijkstraNode {
        public:
-               RRTNode* node = nullptr;
+               RRTNode* p_rrtnode = nullptr;
                unsigned int i = 0;
                bool v = false;
                double d = 0.0;
-               bool vi();
-               DijkstraNode(RRTNode* n);
+               bool vi()
+               {
+                       if (this->v) {
+                               return true;
+                       }
+                       this->v = true;
+                       return false;
+               }
+               DijkstraNode(RRTNode* n) : p_rrtnode(n) {}
        };
        class DijkstraNodeComparator {
        public:
-               int operator() (DijkstraNode const& n1, DijkstraNode const& n2);
+               int operator() (DijkstraNode const& n1, DijkstraNode const& n2)
+               {
+                       return n1.p_rrtnode->cc() > n2.p_rrtnode->cc();
+               }
        };
        class DijkstraNodeBackwardComparator {
        public:
-               int operator() (DijkstraNode const& n1, DijkstraNode const& n2);
+               int operator() (DijkstraNode const& n1, DijkstraNode const& n2)
+               {
+                       return n1.d > n2.d;
+               }
        };
        std::vector<RRTNode*> opath_;
        double ogoal_cc_ = 0.0;
@@ -146,11 +159,11 @@ private:
        void interesting_backward();
        void dijkstra_forward();
        void dijkstra_backward();
-       void compute_path();
+       void compute_path(void) override;
 public:
        RRTExt13();
-       Json::Value json() const;
-       void reset();
+       Json::Value json(void) const override;
+       void reset(void) override;
 };
 
 /*! \brief Reeds & Shepp (build) and Euclidean + abs angle (search).