diff --git a/grid/Grid.h b/grid/Grid.h index f95d74b..a02cc44 100755 --- a/grid/Grid.h +++ b/grid/Grid.h @@ -152,6 +152,11 @@ public: return (hashes.find(uid) != hashes.end()); } + /** get a list of all nodes within the graph */ + const std::vector& getNodes() const { + return nodes; + } + /** get the center-node the given Point belongs to */ const T& getNodeFor(const GridPoint& p) { const UID uid = getUID(p); diff --git a/nav/a-star/AStar.h b/nav/a-star/AStar.h new file mode 100644 index 0000000..131d18f --- /dev/null +++ b/nav/a-star/AStar.h @@ -0,0 +1,119 @@ +#ifndef ASTAR_H +#define ASTAR_H + +#include +#include +#include +#include +#include +#include +#include + +#include "../../grid/Grid.h" + + +template class AStar { + +public: + + + + //dijkstra with priority queue O(E log V) + template static std::vector get(const T* source, const T* destination, Access acc) { + + // track distances from the source to each other node + std::unordered_map distance; + + // track the previous node for each node along the path + std::unordered_map parent; + + // all nodes + const std::vector& nodes = acc.getAllNodes(); + + // priority queue to check which node is to-be-processed next + std::priority_queue, std::vector>, Comparator2> Q; + + // start with infinite distance + for(const auto& node : nodes){ + distance[&node] = std::numeric_limits::max(); + } + + + // start at the source + distance[source] = 0.0f; + Q.push(std::make_pair(source,distance[source])); + + // proceed until there are now new nodes to follow + while(!Q.empty()) { + + // fetch the next-nearest node from the queue + const T* u = Q.top().first; + + // and check whether we reached the destination + if (u == destination) {break;} + + // remove from the Queue + Q.pop(); + + // process all neighbors for the current element + for( const T& v : acc.getNeighbors(*u)) { + + // weight (distance) between the current node and its neighbor + //const float w = ((Point3)v - (Point3)*u).length(); + const float w = acc.getWeightBetween(v, *u); + + // found a better route? + if (distance[&v] > distance[u] + w) { + distance[&v] = distance[u] + w; + parent[&v] = u; + Q.push(std::make_pair(&v, distance[&v] + acc.getHeuristic(v, *source))); + } + + } + } + + + // construct the path + std::vector path; + const T* p = destination; + path.push_back(destination); + + // until we reached the source-node + while (p!=source) { + if (p) { + p = parent[p]; + path.push_back(p); + } else { + return std::vector(); //if no path could be found, just return an empty vector. + } + } + + // done + return path; + + } + + +// template static std::vector getShortestPathAStar(const T* src, const T* dst, Access acc){ + +// std::vector shortestPath; + +// //here we could do some preprocessing. e.g. area of interest of nodes + + +// // call aStar +// shortestPath = aStar(src, dst, acc); + +// return shortestPath; +// } + + class Comparator2 { + public: + int operator() ( const std::pair& p1, const std::pair& p2){ + return p1.second > p2.second; + } + }; + +}; + +#endif // ASTAR_H diff --git a/nav/dijkstra/Dijkstra.h b/nav/dijkstra/Dijkstra.h index c6ac063..3b1f0f0 100644 --- a/nav/dijkstra/Dijkstra.h +++ b/nav/dijkstra/Dijkstra.h @@ -49,7 +49,7 @@ public: // runs until all nodes were evaluated (void) end; - Log::add("Dijkstra", "calculating dijkstra from " + (std::string)*start + " to ALL OTHER nodes", false); + Log::add("Dijkstra", "calculating dijkstra from " + (std::string)*start + " to " + ((end)?((std::string)*end):"ALL OTHER nodes"), true); Log::tick(); // cleanup previous runs @@ -75,10 +75,10 @@ public: DijkstraNode* dnSrc = toBeProcessedNodes.pop(); // when an end is given, stop when end was reached - if (end != nullptr && dnSrc->element == end) {break;} + if (end != nullptr && dnSrc->element == end) {Log::add("Dijkstra", "reached target node"); break;} // when a maximum weight is given, stop when current cum-dist > maxWeight - if (maxWeight != 0 && dnSrc->cumWeight > maxWeight) {break;} + if (maxWeight != 0 && dnSrc->cumWeight > maxWeight) {Log::add("Dijkstra", "reached distance limit"); break;} // visit (and maybe update) each neighbor of the current element for (int i = 0; i < acc.getNumNeighbors(*dnSrc->element); ++i) { @@ -113,8 +113,8 @@ public: } + Log::add("Dijkstra", "processed " + std::to_string(nodes.size()) + " nodes", false); Log::tock(); - Log::add("Dijkstra", "processed " + std::to_string(nodes.size()) + " nodes"); } diff --git a/tests/nav/a-star/TestAStar.cpp b/tests/nav/a-star/TestAStar.cpp new file mode 100644 index 0000000..4a95d8b --- /dev/null +++ b/tests/nav/a-star/TestAStar.cpp @@ -0,0 +1,48 @@ +#ifdef WITH_TESTS + +#include "../../Tests.h" + +#include "../../../grid/Grid.h" +#include "../../../nav/a-star/AStar.h" +#include "../../grid/Plot.h" + +TEST(AStar, build) { + + Grid grid(1); + + int idx1 = grid.add(GP( 0, 0, 0)); + int idx2 = grid.add(GP( 0, 1, 0)); + int idx3 = grid.add(GP( 0,-1, 0)); + int idx4 = grid.add(GP( 1, 0, 0)); + int idx5 = grid.add(GP(-1, 0, 0)); + + grid.connectBiDir(idx1, idx2); + grid.connectBiDir(idx1, idx3); + grid.connectBiDir(idx1, idx4); + grid.connectBiDir(idx1, idx5); + + class TMP { + Grid& grid; + public: + TMP(Grid& grid) : grid(grid) {;} +// int getNumNeighbors(const GP& node) const {return node.getNumNeighbors();} +// const GP* getNeighbor(const GP& node, const int idx) const {return &grid.getNeighbor(node, idx);} + const std::vector& getAllNodes() const {return grid.getNodes();} + decltype(grid.neighbors(GP())) getNeighbors(const GP& node) const {return grid.neighbors(node);} + float getWeightBetween(const GP& n1, const GP& n2) const {return ((Point3)n1 - (Point3)n2).length();} + float getHeuristic(const GP& n1, const GP& n2) const {return std::abs(n1.x_cm - n2.x_cm) + std::abs(n1.y_cm - n2.y_cm);} + } tmp(grid); + + AStar nav; + std::vector vec = nav.get(&grid[0], &grid[4], tmp); + + for (const GP* g : vec) { + std::cout << g << std::endl; + } + + +} + + + +#endif