Rewrite ext13

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

diff --git a/incl/rrtext.hh b/incl/rrtext.hh
index 299f6202c7583756029decbb51d590118e736b8d..3469c08edcd48e05a8c997db00690ea2f54d5160 100644 (file)
--- a/incl/rrtext.hh
+++ b/incl/rrtext.hh
@@ -37,36 +37,32 @@ public:
 
 /*! Use Dijkstra-based path optimization, goal zone for interesting nodes. */
 class RRTExt13 : public virtual RRTS {
-       private:
+private:
+       class DijkstraNode {
        public:
-               void reset();
-               std::vector<RRTNode *> orig_path_;
-               double orig_path_cost_ = 9999;
-               std::vector<RRTNode *> first_optimized_path_;
-               double first_optimized_path_cost_ = 9999;
-               void first_path_optimization();
-               void second_path_optimization();
-               void compute_path();
-               Json::Value json();
-               void json(Json::Value jvi);
-
-               // getter, setter
-               std::vector<RRTNode *> &orig_path()
-               {
-                       return this->orig_path_;
-               };
-               double &orig_path_cost() { return this->orig_path_cost_; }
-               void orig_path_cost(double c) { this->orig_path_cost_ = c; }
-               std::vector<RRTNode *> &first_optimized_path()
-               {
-                       return this->first_optimized_path_;
-               };
-               double &first_optimized_path_cost() {
-                       return this->first_optimized_path_cost_;
-               }
-               void first_optimized_path_cost(double c) {
-                       this->first_optimized_path_cost_ = c;
-               }
+               RRTNode* node = nullptr;
+               unsigned int i = 0;
+               bool v = false;
+               bool vi();
+               DijkstraNode(RRTNode* n);
+       };
+       class DijkstraNodeComparator {
+       public:
+               int operator() (DijkstraNode const& n1, DijkstraNode const& n2);
+       };
+       std::vector<RRTNode*> opath_;
+       double ogoal_cc_ = 0.0;
+       double otime_ = 0.0;
+       std::vector<DijkstraNode> dn_;
+       void pick_interesting();
+       void dijkstra_forward();
+       void dijkstra_backward();
+       void compute_path();
+public:
+       RRTExt13();
+       Json::Value json() const;
+       void json(Json::Value jvi);
+       void reset();
 };
 
 /*! \brief Different `steer` procedures.