Add default sampling info setter

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

diff --git a/incl/rrtbase.h b/incl/rrtbase.h
index 4a67e9a2bea6247449666ead68a4a4a68d776e5e..9b17d44e673c772bc1d7501f215fb8940b2565eb 100644 (file)
--- a/incl/rrtbase.h
+++ b/incl/rrtbase.h
@@ -18,24 +18,44 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #ifndef RRTBASE_H
 #define RRTBASE_H
 
+#include <array>
 #include <chrono>
 #include <cmath>
 #include <pthread.h>
+#include <queue>
+#include <random>
 #include <vector>
 #include "obstacle.h"
 #include "rrtnode.h"
-#include "sample.h"
+#include "slotplanner.h"
 
 #define NOFNODES 20000
 
 #define IXSIZE 100
-#define IXSTEP (1.0 * ((VMAX) - (VMIN)) / IXSIZE)
-#define IXI(x) ({ __typeof__ (x) _x = (x); \
-                (int) floor(_x / IXSTEP); })
 #define IYSIZE 100
-#define IYSTEP (1.0 * ((HMAX) - (HMIN)) / IYSIZE)
-#define IYI(y) ({ __typeof__ (y) _y = (y); \
-                (int) floor(_y / IYSTEP); })
+
+#define GLVERTEX(n) ((n)->x() * glplwscale), ((n)->y() * glplhscale)
+
+// Nearest neighbor
+struct mcnn { // min-cost nearest neighbour
+        float mc;
+        RRTNode *nn;
+};
+#pragma omp declare reduction \
+        (minn: struct mcnn: omp_out = \
+         omp_in.mc < omp_out.mc ? omp_in : omp_out) \
+       initializer \
+       (omp_priv(omp_orig))
+
+// Near vertices
+#define GAMMA_RRTSTAR(card_V) ({ \
+       __typeof__ (card_V) _card_V = (card_V); \
+       pow(log(_card_V) / _card_V, 1.0/3.0); \
+})
+
+# pragma omp declare reduction \
+        (merge: std::vector<RRTNode *>: \
+         omp_out.insert(omp_out.end(), omp_in.begin(), omp_in.end()))
 
 class Cell {
         private:
@@ -57,9 +77,11 @@ class RRTBase {
         private:
                 RRTNode *root_ = nullptr;
                 RRTNode *goal_ = nullptr;
+                std::vector<RRTNode *> goals_;
 
                 std::vector<RRTNode *> nodes_;
                 std::vector<RRTNode *> dnodes_;
+                std::queue<RRTNode *> firsts_;
                 PolygonObstacle frame_;
                 std::vector<RRTNode *> samples_;
                 std::vector<CircleObstacle> *cobstacles_;
@@ -74,9 +96,19 @@ class RRTBase {
                 std::vector<std::vector<RRTEdge *>> rlog_; // log tree
                 std::vector<float> slog_; // seconds of trajectories
                 std::vector<std::vector<RRTNode *>> tlog_; // trajectories
+                std::vector<RRTNode *> slot_cusp_; // cusp nodes in slot
+        protected:
+                std::default_random_engine gen_;
+                std::normal_distribution<float> ndx_;
+                std::normal_distribution<float> ndy_;
+                std::normal_distribution<float> ndh_;
         public:
                 const float GOAL_FOUND_DISTANCE = 0.2;
                 const float GOAL_FOUND_ANGLE = M_PI / 32;
+                float HMIN = -20;
+                float HMAX = 20;
+                float VMIN = -5;
+                float VMAX = 30;
 
                 ~RRTBase();
                 RRTBase();
@@ -85,11 +117,13 @@ class RRTBase {
                 // getter
                 RRTNode *root();
                 RRTNode *goal();
+                std::vector<RRTNode *> &goals();
                 std::vector<RRTNode *> &nodes();
                 std::vector<RRTNode *> &dnodes();
+                std::queue<RRTNode *> &firsts();
                 PolygonObstacle &frame();
                 std::vector<RRTNode *> &samples();
-                std::vector<RRTNode *> iy_[IYSIZE];
+                std::array<std::vector<RRTNode *>, IYSIZE> iy_;
                 Cell ixy_[IXSIZE][IYSIZE];
                 std::vector<CircleObstacle> *co();
                 std::vector<SegmentObstacle> *so();
@@ -98,12 +132,15 @@ class RRTBase {
                 std::vector<std::vector<RRTEdge *>> &rlog();
                 std::vector<float> &slog();
                 std::vector<std::vector<RRTNode *>> &tlog();
+                std::vector<RRTNode *> &slot_cusp();
                 bool goal_found();
                 float elapsed();
+                std::vector<RRTNode *> traj_cusp();
 
                 // setter
                 void root(RRTNode *node);
                 void goal(RRTNode *node);
+                void goals(std::vector<RRTNode *> g);
                 bool logr(RRTNode *root);
                 float ocost(RRTNode *n);
                 bool tlog(std::vector<RRTNode *> t);
@@ -115,12 +152,17 @@ class RRTBase {
                 bool add_iy(RRTNode *n);
                 bool add_ixy(RRTNode *n);
                 bool goal_found(bool f);
+                void slot_cusp(std::vector<RRTNode *> sc);
 
                 // other
                 bool glplot();
                 bool goal_found(
                                 RRTNode *node,
                                 float (*cost)(RRTNode *, RRTNode *));
+                bool goal_found(
+                        RRTNode *node,
+                        RRTNode *goal
+                );
                 bool collide(RRTNode *init, RRTNode *goal);
                 bool optp_dijkstra(
                                 std::vector<RRTNode *> &cusps,
@@ -135,8 +177,12 @@ class RRTBase {
                 bool rebase(RRTNode *nr);
                 std::vector<RRTNode *> findt();
                 std::vector<RRTNode *> findt(RRTNode *n);
+                int XI(RRTNode *n);
+                int YI(RRTNode *n);
 
                 // RRT Framework
+                void defaultSamplingInfo();
+                void setSamplingInfo(SamplingInfo si);
                 RRTNode *sample();
                 float cost(RRTNode *init, RRTNode *goal);
                 RRTNode *nn(RRTNode *rs);