Move GL-relevant macros

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

diff --git a/incl/rrtbase.h b/incl/rrtbase.h
index 4ece3a7ab98358e2f9f94c6f86ef93c28a4d0815..765b4958fa2789f60950a42a51346d9c36106cdd 100644 (file)
--- a/incl/rrtbase.h
+++ b/incl/rrtbase.h
@@ -18,6 +18,7 @@ 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>
@@ -32,13 +33,17 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #define NOFNODES 20000
 
 #define IXSIZE 100
-#define IXSTEP (1.0 * ((VMAX) - (VMIN)) / IXSIZE)
+#define IXSTEP (1.0 * ((HMAX) - (HMIN)) / IXSIZE)
 #define IXI(x) ({ __typeof__ (x) _x = (x); \
                 std::abs((int) floor(_x / IXSTEP)); })
 #define IYSIZE 100
-#define IYSTEP (1.0 * ((HMAX) - (HMIN)) / IYSIZE)
-#define IYI(y) ({ __typeof__ (y) _y = (y); \
-                std::abs((int) floor(_y / IYSTEP)); })
+#define IYSTEP (1.0 * ((VMAX) - (VMIN)) / IYSIZE)
+#define IYI(y) ({ \
+        __typeof__ (y) _y = (y); \
+        (int) floor((_y - VMIN) / IYSTEP); \
+})
+
+#define GLVERTEX(n) ((n)->x() * glplwscale), ((n)->y() * glplhscale)
 
 class Cell {
         private:
@@ -80,11 +85,18 @@ class RRTBase {
                 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();
@@ -99,7 +111,7 @@ class RRTBase {
                 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();
@@ -153,10 +165,11 @@ 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
-                SamplingInfo samplingInfo_;
-                bool useSamplingInfo_ = false;
+                void setSamplingInfo(SamplingInfo si);
                 RRTNode *sample();
                 float cost(RRTNode *init, RRTNode *goal);
                 RRTNode *nn(RRTNode *rs);