Small improvements

PathPlanning · Nov 9, 2016 · 5ae890d · 5ae890d
1 parent 7b68619
commit 5ae890d
Show file tree

Hide file tree

Showing 11 changed files with 269 additions and 522 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -39,5 +39,5 @@ set(SOURCE_FILES
         dijkstra.cpp
         dijkstra.h
         bfs.cpp
-        bfs.h)
+        bfs.h Queues.cpp Queues.h)
 add_executable(Astar_heights ${SOURCE_FILES})
diff --git a/Queues.cpp b/Queues.cpp
@@ -0,0 +1,161 @@
+#include "Queues.h"
+#include "node.h"
+#include "gl_const.h"
+
+bool iOpen::less(const Node &x, const Node &y) const {
+    if (x.F == y.F) {
+        switch (breaking_tie) {
+            case CN_SP_BT_GMIN:
+                return x.g < y.g;
+            case CN_SP_BT_GMAX:
+            default:
+                return x.g > y.g;
+        }
+    }
+    return x.F < y.F;
+}
+
+SortedList::SortedList(size_t size, int breakingtie) : data(size), size_(0), min_pos(size) {
+    breaking_tie = breakingtie;
+}
+
+size_t SortedList::size() const {
+    return size_;
+}
+
+bool SortedList::empty() const {
+    return (size_ == 0);
+}
+
+bool SortedList::Insert(const Node &newNode) {
+    std::list<Node>::iterator iter, pos;
+
+    bool posFound = false;
+
+    pos = data[newNode.i].end();
+
+    if (data[newNode.i].empty()) {
+        data[newNode.i].push_back(newNode);
+        ++size_;
+        return true;
+    }
+
+    for (iter = data[newNode.i].begin(); iter != data[newNode.i].end(); ++iter) {
+        if (!posFound && !less(*iter, newNode)) {
+            pos = iter;
+            posFound = true;
+        }
+
+        if (iter->j == newNode.j && iter->z == newNode.z) {
+            if (newNode.F >= iter->F) {
+                return false;
+            } else {
+                if (pos == iter) {
+                    iter->g = newNode.g;
+                    iter->F = newNode.F;
+                    iter->H = newNode.H;
+                    return true;
+                }
+                data[newNode.i].erase(iter);
+                --size_;
+                break;
+            }
+        }
+    }
+    ++size_;
+    data[newNode.i].insert(pos, newNode);
+    return true;
+}
+
+Node SortedList::FindMin() const {
+    Node min;
+    min.F = -1;
+    for (size_t i = 0; i < data.size(); i++) {
+        if (!data[i].empty())
+            if (min.F == -1 || less(*data[i].begin(), min)) {
+                min = *data[i].begin();
+                min_pos = i;
+            }
+    }
+    return min;
+}
+
+void SortedList::DeleteMin() {
+    if (min_pos >= data.size()) {
+        FindMin();
+    }
+    data[min_pos].pop_front();
+    --size_;
+    min_pos = data.size();
+}
+
+ClusteredSets::ClusteredSets(size_t size, int breakingtie) : loc_mins(size), data(size), size_(0), min_pos(size) {
+    breaking_tie = breakingtie;
+}
+
+size_t ClusteredSets::size() const {
+    return size_;
+}
+
+bool ClusteredSets::empty() const {
+    return (size_ == 0);
+}
+
+bool ClusteredSets::Insert(const Node &NewNode) {
+    bool node_found = false;
+    bool updated = false;
+
+    auto pos = data[NewNode.i].find(NewNode);
+
+    if (pos != data[NewNode.i].end()) {
+        node_found = true;
+        if (NewNode.g < pos->g) {
+            data[NewNode.i].erase(pos);
+            data[NewNode.i].insert(NewNode);
+            updated = true;
+        } else {
+            return false;
+        }
+    }
+
+    if (!node_found) {
+        updated = true;
+        data[NewNode.i].insert(NewNode);
+        ++size_;
+    }
+    if (data[NewNode.i].size() == 1 || less(NewNode, loc_mins[NewNode.i])) {
+        loc_mins[NewNode.i] = NewNode;
+    }
+    return updated;
+}
+
+Node ClusteredSets::FindMin() const {
+    for (min_pos = 0; data[min_pos].empty(); ++min_pos) {}
+    for (size_t i = min_pos + 1; i < loc_mins.size(); ++i) {
+        if (!data[i].empty() && less(loc_mins[i], loc_mins[min_pos])) {
+            min_pos = i;
+        }
+    }
+    return loc_mins[min_pos];
+}
+
+void ClusteredSets::DeleteMin() {
+    if (min_pos == loc_mins.size()) {
+        FindMin();
+    }
+
+    data[min_pos].erase(loc_mins[min_pos]);
+    Node min;
+    if (!data[min_pos].empty()) {
+        auto it = data[min_pos].begin();
+        min = *(it++);
+        for (; it != data[min_pos].end(); ++it) {
+            if (less(*it, min)) {
+                min = *it;
+            }
+        }
+
+        loc_mins[min_pos] = min;
+    }
+    min_pos = loc_mins.size();
+}
diff --git a/Queues.h b/Queues.h
@@ -0,0 +1,74 @@
+#ifndef QUEUES_H
+#define QUEUES_H
+
+#include <list>
+#include <vector>
+#include <unordered_set>
+
+#include "node.h"
+
+class iOpen {
+protected:
+    virtual bool less(const Node &x, const Node &y) const;
+    int breaking_tie;
+public:
+    iOpen() {};
+
+    virtual bool Insert(const Node &NewNode) = 0;
+
+    virtual Node FindMin() const = 0;
+
+    virtual void DeleteMin() = 0;
+
+    virtual size_t size() const = 0;
+
+    virtual bool empty() const = 0;
+};
+
+class SortedList : public iOpen {
+private:
+    std::vector<std::list<Node>> data;
+    size_t size_;
+
+    mutable size_t min_pos;
+
+public:
+    SortedList() = default;
+
+    SortedList(size_t size, int brakingtie);
+
+    virtual bool Insert(const Node &NewNode);
+
+    virtual Node FindMin() const;
+
+    virtual void DeleteMin();
+
+    virtual size_t size() const;
+
+    virtual bool empty() const;
+};
+
+class ClusteredSets : public iOpen {
+private:
+    std::vector<Node> loc_mins;
+    std::vector<std::unordered_set<Node>> data;
+    size_t size_;
+    mutable size_t min_pos;
+
+public:
+    ClusteredSets() = default;
+
+    ClusteredSets(size_t size, int breakingtie);
+
+    virtual bool Insert(const Node &NewNode);
+
+    virtual Node FindMin() const;
+
+    virtual void DeleteMin();
+
+    virtual size_t size() const;
+
+    virtual bool empty() const;
+};
+
+#endif
diff --git a/astar.cpp b/astar.cpp
@@ -52,58 +52,5 @@ double Astar::computeHFromCellToCell(int start_i, int start_j, int start_h, int
 }
 
 void Astar::addOpen(Node newNode) {
-    std::list<Node>::iterator iter, pos;
-
-    bool posFound = false;
-
-    pos = open[newNode.i].List.end();
-
-    if (open[newNode.i].List.empty()) {
-        open[newNode.i].List.push_back(newNode);
-        openSize++;
-        return;
-    }
-
-    for (iter = open[newNode.i].List.begin(); iter != open[newNode.i].List.end(); ++iter) {
-        if ((iter->F >= newNode.F) && (!posFound)) {
-            if (iter->F == newNode.F) {
-                switch (breakingties) {
-                    case CN_SP_BT_GMAX: {
-                        if (newNode.g >= iter->g) {
-                            pos = iter;
-                            posFound = true;
-                        }
-
-                    }
-                    case CN_SP_BT_GMIN: {
-                        if (newNode.g <= iter->g) {
-                            pos = iter;
-                            posFound = true;
-                        }
-                    }
-                }
-            } else {
-                pos = iter;
-                posFound = true;
-            }
-        }
-
-        if (((iter->i) == newNode.i) && (iter->j) == newNode.j && iter->z == newNode.z) {
-            if (newNode.F >= iter->F) {
-                return;
-            } else {
-                if (pos == iter) {
-                    iter->F = newNode.F;
-                    iter->g = newNode.g;
-                    iter->parent = newNode.parent;
-                    return;
-                }
-                open[newNode.i].List.erase(iter);
-                openSize--;
-                break;
-            }
-        }
-    }
-    openSize++;
-    open[newNode.i].List.insert(pos, newNode);
+    open->Insert(newNode);
 }
diff --git a/astar.h b/astar.h
@@ -14,7 +14,7 @@ class Astar : public ISearch
     protected:
         double computeHFromCellToCell(int start_i, int start_j, int start_h, int fin_i, int fin_j, int fin_h,
                                                const EnvironmentOptions &options);
-        void    addOpen(Node newNode);
+        inline void    addOpen(Node newNode);
 };
 
 #endif
diff --git a/dijkstra.cpp b/dijkstra.cpp
@@ -12,41 +12,3 @@ double Dijkstra::computeHFromCellToCell(int start_i, int start_j, int start_h, i
                                         const EnvironmentOptions &options) {
     return 0;
 }
-
-void Dijkstra::addOpen(Node newNode) {
-    bool posFound = false;
-
-    auto pos = open[newNode.i].List.end();
-
-    if (open[newNode.i].List.empty()) {
-        open[newNode.i].List.push_back(newNode);
-        ++openSize;
-        return;
-    }
-
-    for (auto iter = open[newNode.i].List.begin(); iter != open[newNode.i].List.end(); ++iter) {
-        if ((iter->g >= newNode.g) && (!posFound)) {
-            pos = iter;
-            posFound = true;
-        }
-
-        if (((iter->i) == newNode.i) && (iter->j) == newNode.j && (iter->z) == newNode.z) {
-            if (newNode.g >= iter->g)
-                return;
-            else {
-                if (pos == iter) {
-                    iter->F = newNode.F;
-                    iter->g = newNode.g;
-                    iter->parent = newNode.parent;
-                    return;
-                }
-                open[newNode.i].List.erase(iter);
-                --openSize;
-                break;
-            }
-        }
-    }
-
-    open[newNode.i].List.insert(pos, newNode);
-    ++openSize;
-}
diff --git a/dijkstra.h b/dijkstra.h
@@ -1,17 +1,15 @@
 #ifndef DIJKSTRA_H
 #define DIJKSTRA_H
 
+#include "astar.h"
 #include "gl_const.h"
-#include "isearch.h"
 
-class Dijkstra : public ISearch {
+class Dijkstra : public Astar {
 public:
     Dijkstra(int i);
 
     ~Dijkstra(void);
 
-    virtual void addOpen(Node newNode);
-
     virtual double computeHFromCellToCell(int start_i, int start_j, int start_h, int fin_i, int fin_j, int fin_h,
                                           const EnvironmentOptions &options);
 };