Merge branch 'release/0.7.0'

[hubacji1/iamcar.git] / incl / rrtplanner.h

diff --git a/incl/rrtplanner.h b/incl/rrtplanner.h
index a6698fdc97b4e0cac9da30db3ba5bbe7254fcb17..4910d13831d9d0cd5df2186fe6c19cf2f3df755c 100644 (file)
--- a/incl/rrtplanner.h
+++ b/incl/rrtplanner.h
@@ -28,78 +28,68 @@ class LaValle1998: public RRTBase {
         public:
                 //using RRTBase::RRTBase;
                 LaValle1998(RRTNode *init, RRTNode *goal);
-
-                // RRT framework
-                RRTNode *(*nn)(
-                                RRTNode *root,
-                                RRTNode *node,
-                                float (*cost)(RRTNode *, RRTNode *));
-                RRTNode *(*sample)();
-                std::vector<RRTNode *> (*steer)(
-                                RRTNode *init,
-                                RRTNode *goal);
-                float (*cost)(RRTNode *init, RRTNode *goal);
                 bool next();
 };
 
 class Kuwata2008: public RRTBase {
         public:
                 Kuwata2008(RRTNode *init, RRTNode *goal);
-
-                // RRT framework
-                RRTNode *(*nn)(
-                                RRTNode *root,
-                                RRTNode *node,
-                                float (*cost)(RRTNode *, RRTNode *));
-                RRTNode *(*sample)();
-                std::vector<RRTNode *> (*steer)(
-                                RRTNode *init,
-                                RRTNode *goal);
-                float (*cost)(RRTNode *init, RRTNode *goal);
                 bool next();
 };
 
 class Karaman2011: public RRTBase {
+        protected:
+                bool connect(
+                                RRTNode *pn,
+                                RRTNode *ns,
+                                std::vector<RRTNode *> nvs);
+                bool rewire(std::vector<RRTNode *> nvs, RRTNode *ns);
         public:
+                Karaman2011();
                 Karaman2011(RRTNode *init, RRTNode *goal);
-
-                // RRT framework
-                RRTNode *(*nn)(
-                                RRTNode *root,
-                                RRTNode *node,
-                                float (*cost)(RRTNode *, RRTNode *));
-                std::vector<RRTNode *> (*nv)(
-                                RRTNode *root,
-                                RRTNode *node,
-                                float (*cost)(RRTNode *, RRTNode *),
-                                float dist);
-                RRTNode *(*sample)();
-                std::vector<RRTNode *> (*steer)(
-                                RRTNode *init,
-                                RRTNode *goal);
-                float (*cost)(RRTNode *init, RRTNode *goal);
                 bool next();
 };
 
 class T1: public RRTBase {
         public:
                 T1(RRTNode *init, RRTNode *goal);
+                bool next();
+};
+
+class T2: public Karaman2011 {
+        public:
+                using Karaman2011::Karaman2011;
+                bool next();
+                float goal_cost();
+};
 
-                // RRT framework
-                RRTNode *(*nn)(
-                                RRTNode *root,
-                                RRTNode *node,
-                                float (*cost)(RRTNode *, RRTNode *));
-                std::vector<RRTNode *> (*nv)(
-                                RRTNode *root,
-                                RRTNode *node,
-                                float (*cost)(RRTNode *, RRTNode *),
-                                float dist);
-                RRTNode *(*sample)();
-                std::vector<RRTNode *> (*steer)(
-                                RRTNode *init,
-                                RRTNode *goal);
-                float (*cost)(RRTNode *init, RRTNode *goal);
+class T3: public RRTBase {
+        protected:
+        public:
+                T2 p_root_;
+                T2 p_goal_;
+                ~T3();
+                T3();
+                T3(RRTNode *init, RRTNode *goal);
+                bool next();
+                bool link_obstacles(
+                                std::vector<CircleObstacle> *cobstacles,
+                                std::vector<SegmentObstacle> *sobstacles);
+                bool connecttrees(RRTNode *rn, RRTNode *gn);
+                bool overlaptrees(RRTNode **ron, RRTNode **gon);
+};
+
+class Klemm2015: public Karaman2011 {
+        private:
+                RRTNode *orig_root_ = nullptr;
+                RRTNode *orig_goal_ = nullptr;
+        protected:
+                int extendstar1(RRTNode *rs, RRTNode **xn);
+                int extendstarC(RRTNode *rs);
+                int connectstar(RRTNode *x);
+                void swap();
+        public:
+                Klemm2015(RRTNode *init, RRTNode *goal);
                 bool next();
 };