Theta* on map of heights

ASearch.pro

@@ -8,7 +8,7 @@ TARGET = ASearch
 CONFIG   += console
 CONFIG   -= app_bundle
 TEMPLATE = app
-QMAKE_CXXFLAGS += -std=c++11
+QMAKE_CXXFLAGS += -std=c++11 -O3
 
 win32 {
 QMAKE_LFLAGS += -static -static-libgcc -static-libstdc++
@@ -28,7 +28,9 @@ SOURCES += \
     config.cpp \
     astar.cpp \
     asearch.cpp \
-    environmentoptions.cpp
+    environmentoptions.cpp \
+    Bresenham.cpp \
+    theta.cpp
 
 HEADERS += \
     tinyxml.h \
@@ -44,4 +46,6 @@ HEADERS += \
     config.h \
     astar.h \
     searchresult.h \
-    environmentoptions.h
+    environmentoptions.h \
+    Bresenham.h \
+    theta.h

Bresenham.cpp

@@ -0,0 +1,144 @@
+#include "Bresenham.h"
+#include "node.h"
+#include <algorithm>
+#include <stdexcept>
+
+void iBresenham::bresenham3d(int x1, int y1, int z1, const int x2, const int y2, const int z2) {
+
+    int i, dx, dy, dz, l, m, n, x_inc, y_inc, z_inc, err_1, err_2, dx2, dy2, dz2;
+    int point[3];
+
+    point[0] = x1;
+    point[1] = y1;
+    point[2] = z1;
+    dx = x2 - x1;
+    dy = y2 - y1;
+    dz = z2 - z1;
+    if (dx == 0) {
+        x_inc = 0;
+    } else if (dx < 0) {
+        x_inc = -1;
+    } else {
+        x_inc = 1;
+    }
+    l = abs(dx);
+    if (dy == 0) {
+        y_inc = 0;
+    } else if (dy < 0) {
+        y_inc = -1;
+    } else {
+        y_inc = 1;
+    }
+    m = abs(dy);
+    if (dz == 0) {
+        z_inc = 0;
+    } else if (dz < 0) {
+        z_inc = -1;
+    } else {
+        z_inc = 1;
+    }
+    n = abs(dz);
+    dx2 = l << 1;
+    dy2 = m << 1;
+    dz2 = n << 1;
+
+    if ((l >= m) && (l >= n)) {
+        err_1 = dy2 - l;
+        err_2 = dz2 - l;
+        for (i = 0; i < l; i++) {
+            if (!ProcessPoint(point[0], point[1], point[2])) {
+                return;
+            }
+            if (err_1 > 0) {
+                point[1] += y_inc;
+                err_1 -= dx2;
+            }
+            if (err_2 > 0) {
+                point[2] += z_inc;
+                err_2 -= dx2;
+            }
+            err_1 += dy2;
+            err_2 += dz2;
+            point[0] += x_inc;
+        }
+    } else if ((m >= l) && (m >= n)) {
+        err_1 = dx2 - m;
+        err_2 = dz2 - m;
+        for (i = 0; i < m; i++) {
+            if (!ProcessPoint(point[0], point[1], point[2])) {
+                return;
+            }
+            if (err_1 > 0) {
+                point[0] += x_inc;
+                err_1 -= dy2;
+            }
+            if (err_2 > 0) {
+                point[2] += z_inc;
+                err_2 -= dy2;
+            }
+            err_1 += dx2;
+            err_2 += dz2;
+            point[1] += y_inc;
+        }
+    } else {
+        err_1 = dy2 - n;
+        err_2 = dx2 - n;
+        for (i = 0; i < n; i++) {
+            if (!ProcessPoint(point[0], point[1], point[2])) {
+                return;
+            }
+            if (err_1 > 0) {
+                point[1] += y_inc;
+                err_1 -= dz2;
+            }
+            if (err_2 > 0) {
+                point[0] += x_inc;
+                err_2 -= dz2;
+            }
+            err_1 += dy2;
+            err_2 += dx2;
+            point[2] += z_inc;
+        }
+    }
+    if (!ProcessPoint(point[0], point[1], point[2])) {
+        return;
+    }
+}
+
+LineOfSight::LineOfSight(const Map& init_map) : force_stopped(false), map(init_map) {}
+
+bool LineOfSight::ProcessPoint(int i, int j, int h) {
+    if (map.CellIsObstacle(i, j, h)) {
+        force_stopped = true;
+        return false;
+    }
+    return true;
+}
+
+bool LineOfSight::line_of_sight(int i0, int j0, int h0, int i1, int j1, int h1) {
+    bresenham3d(i0, j0, h0, i1, j1, h1);
+    return !force_stopped;
+}
+
+bool LineOfSight::line_of_sight(const Node &from, const Node &to) {
+    return line_of_sight(from.i, from.j, from.z, to.i, to.j, to.z);
+}
+
+Liner::Liner(const Map &init_map, std::list<Node> *init_path) : map(init_map), path(init_path) {}
+
+bool Liner::ProcessPoint(int i, int j, int h) {
+    Node newNode;
+    newNode.i = i;
+    newNode.j = j;
+    newNode.z = h;
+    path->push_back(newNode);
+    return true;
+}
+
+void Liner::append_line(int i0, int j0, int h0, int i1, int j1, int h1) {
+    bresenham3d(i0, j0, h0, i1, j1, h1);
+}
+
+void Liner::append_line(const Node &from, const Node &to) {
+    bresenham3d(from.i, from.j, from.z, to.i, to.j, to.z);
+}

Bresenham.h

@@ -0,0 +1,42 @@
+#ifndef BRESENHAM_H
+#define BRESENHAM_H
+
+#include "map.h"
+#include "node.h"
+
+#include <list>
+
+class iBresenham {
+protected:
+    void bresenham3d(int x1, int y1, int z1, const int x2, const int y2, const int z2); // 3D Bresenham algorithm
+    virtual bool ProcessPoint(int x, int y, int z) = 0; // Processes point in line. Return false, if line processing should be stopped
+};
+
+class LineOfSight : public iBresenham {
+private:
+    bool force_stopped;
+    const Map& map;
+protected:
+    virtual bool ProcessPoint(int i, int j, int h);
+
+public:
+    LineOfSight(const Map&);
+    bool line_of_sight(int i0, int j0, int h0, int i1, int j1, int h1);
+    bool line_of_sight(const Node& from, const Node& to);
+};
+
+class Liner : public iBresenham {
+private:
+    const Map& map;
+    std::list<Node> *path;
+
+protected:
+    virtual bool ProcessPoint(int i, int j, int h);
+
+public:
+    Liner(const Map&, std::list<Node> *init_path);
+    void append_line(int i0, int j0, int h0, int i1, int j1, int h1);
+    void append_line(const Node &from, const Node &to);
+};
+
+#endif //BRESENHAM_H

CMakeLists.txt

@@ -32,5 +32,8 @@ set(SOURCE_FILES
         astar.h
         searchresult.h
         environmentoptions.h
-        )
+        Bresenham.cpp
+        Bresenham.h
+        theta.cpp
+        theta.h)
 add_executable(Astar_heights ${SOURCE_FILES})

isearch.cpp

@@ -52,6 +52,8 @@ SearchResult ISearch::startSearch(ILogger *Logger, const Map &map, const Environ
     int closeSize = 0;
     bool pathfound = false;
     const Node *curIt;
+    std::vector<Node> successors;
+    successors.reserve(26); // Usually every node has no more than 26 successors
     while (!stopCriterion()) {
         curNode = findMin(map.height);
         curIt = &(*(close.insert(curNode).first));
@@ -64,7 +66,8 @@ SearchResult ISearch::startSearch(ILogger *Logger, const Map &map, const Environ
             break;
         }
 
-        std::list<Node> successors = findSuccessors(curNode, map, options);
+        successors.resize(0);
+        findSuccessors(curNode, map, options, successors);
         for (auto it = successors.begin(); it != successors.end(); ++it) {
             it->parent = curIt;
             it->H = computeHFromCellToCell(it->i, it->j, it->z, map.goal_i, map.goal_j, map.goal_h, options);
@@ -124,9 +127,8 @@ Node ISearch::findMin(int size) {
 
 }
 
-std::list<Node> ISearch::findSuccessors(Node curNode, const Map &map, const EnvironmentOptions &options) {
+void ISearch::findSuccessors(Node curNode, const Map &map, const EnvironmentOptions &options, std::vector<Node> &output) {
     Node newNode;
-    std::list<Node> successors;
     for (int i = -1; i <= 1; ++i) {
         for (int j = -1; j <= 1; ++j) {
             for (int h = -1; h <= 1; ++h) {
@@ -148,13 +150,12 @@ std::list<Node> ISearch::findSuccessors(Node curNode, const Map &map, const Envi
                     if (close.find(newNode) == close.end()) {
                         newNode.g = curNode.g + MoveCost(curNode.i, curNode.j, curNode.z, curNode.i + i, curNode.j + j,
                                                          curNode.z + h, options);
-                        successors.push_front(newNode);
+                        output.push_back(newNode);
                     }
                 }
             }
         }
     }
-    return successors;
 }
 
 void ISearch::makePrimaryPath(Node curNode) {

isearch.h

@@ -29,16 +29,16 @@ class ISearch {
     virtual void addOpen(Node newNode) = 0; //êàæäûé ïîèñê ïî ñâîåìó äîáàâëÿåò âåðøèíû â ñïèñîê OPEN
     virtual double computeHFromCellToCell(int start_i, int start_j, int start_h, int fin_i, int fin_j, int fin_h,
                                           const EnvironmentOptions &options) = 0; //äëÿ Äåéêñòðû è BFS ýòîò ìåòîä âñåãäà âîçâðàùàåò íîëü
-    virtual std::list<Node> findSuccessors(Node curNode, const Map &map,
-                                           const EnvironmentOptions &options);//ìåòîä, êîòîðûé èùåò ñîñåäåé òåêóùåé âåðøèíû, óäîâëåòâîðÿþùèå ïàðàìåòðàì ïîèñêà
+    virtual void findSuccessors(Node curNode, const Map &map, const EnvironmentOptions &options, std::vector<Node> &output);//ìåòîä, êîòîðûé èùåò ñîñåäåé òåêóùåé âåðøèíû, óäîâëåòâîðÿþùèå ïàðàìåòðàì ïîèñêà
     virtual void makePrimaryPath(Node curNode);//ñòðîèò ïóòü ïî ññûëêàì íà ðîäèòåëÿ
     virtual void makeSecondaryPath(const Map &map,
-                                   Node curNode);//ðàçáèâàåò íàéäåííûé ïóòü íà ñåêöèè, ñîäåðæàùèå òîëüêî ïðÿìûå ó÷àñòêè
+                                   Node curNode);
     virtual Node resetParent(Node current, Node parent, const Map &map,
                              const EnvironmentOptions &options) { return current; }//ìåíÿåò ðîäèòåëÿ, íóæåí äëÿ àëãîðèòìà Theta*
     virtual bool stopCriterion();
 
     SearchResult sresult; //ðåçóëüòàò ïîèñêà
+    // TODO correct path storage. Better to place them on the heap.
     NodeList lppath, hppath; //ñïèñêè OPEN, CLOSE è ïóòü
     Node lastnode;
     std::unordered_set<Node> close;

mission.cpp

@@ -3,7 +3,7 @@
 #include "astar.h"
 //#include "bfs.h"
 //#include "dijkstra.h"
-//#include "theta.h"
+#include "theta.h"
 #include "xmllogger.h"
 #include "gl_const.h"
 
@@ -56,7 +56,7 @@ void Mission::createEnvironmentOptions()
 
 void Mission::createSearch()
 {
-    search = new Astar(config.SearchParams[CN_SP_HW], config.SearchParams[CN_SP_BT], config.SearchParams[CN_SP_SL], map.height);
+    search = new Theta(config.SearchParams[CN_SP_HW], config.SearchParams[CN_SP_BT], config.SearchParams[CN_SP_SL], map.height);
     /*if (config.SearchParams[CN_SP_ST] == CN_SP_ST_BFS)
     {
         search = new BFS(map.height);