Merge branch 'feature/refactor-nn'

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

diff --git a/incl/rrtbase.h b/incl/rrtbase.h
index 501a024866e514eb9b264b48c51bfd79a9a9c6bf..44dc06de2784b6ac5fc5022c64adfdebdbe46849 100644 (file)
--- a/incl/rrtbase.h
+++ b/incl/rrtbase.h
@@ -27,22 +27,36 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #include <vector>
 #include "obstacle.h"
 #include "rrtnode.h"
-#include "sample.h"
 #include "slotplanner.h"
 
 #define NOFNODES 20000
 
 #define IXSIZE 100
-#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 * ((VMAX) - (VMIN)) / IYSIZE)
-#define IYI(y) ({ \
-        __typeof__ (y) _y = (y); \
-        (int) floor((_y - VMIN) / 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:
                 std::vector<RRTNode *> nodes_;
@@ -91,6 +105,10 @@ class RRTBase {
         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();
@@ -159,6 +177,8 @@ 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 setSamplingInfo(SamplingInfo si);