134 lines
3.2 KiB
C++
134 lines
3.2 KiB
C++
#ifndef ASTAR_H
|
|
#define ASTAR_H
|
|
|
|
#include <iostream>
|
|
#include <vector>
|
|
#include <algorithm>
|
|
#include <limits>
|
|
#include <queue>
|
|
#include <unordered_map>
|
|
#include <cassert>
|
|
|
|
#include "../../grid/Grid.h"
|
|
|
|
|
|
template <typename T> class AStar {
|
|
|
|
public:
|
|
|
|
#define LE_MAX 500000
|
|
|
|
//dijkstra with priority queue O(E log V)
|
|
template <typename Access>
|
|
static float get(const T* source, const T* destination, Access acc) {
|
|
|
|
// track distances from the source to each other node
|
|
//std::unordered_map<const T*, float> distance;
|
|
|
|
float distance[LE_MAX];
|
|
|
|
|
|
// track the previous node for each node along the path
|
|
//std::unordered_map<const T*, const T*> parent;
|
|
const T* parent[LE_MAX];
|
|
|
|
// all nodes
|
|
//const std::vector<T>& nodes = acc.getAllNodes();
|
|
|
|
// priority queue to check which node is to-be-processed next
|
|
std::priority_queue<std::pair<T*,float>, std::vector<std::pair<const T*,float>>, Comparator2> Q;
|
|
|
|
// start with infinite distance
|
|
// for(const auto& node : nodes){
|
|
// distance[node.getIdx()] = std::numeric_limits<float>::max();
|
|
// }
|
|
std::fill_n(distance, LE_MAX, std::numeric_limits<float>::max());
|
|
|
|
|
|
// std::cout << (std::string)*source << std::endl;
|
|
// std::cout << (std::string)*destination << std::endl;
|
|
// int iter = 0;
|
|
|
|
// start at the source
|
|
distance[source->getIdx()] = 0.0f;
|
|
Q.push(std::make_pair(source,distance[source->getIdx()]));
|
|
|
|
// proceed until there are now new nodes to follow
|
|
while(!Q.empty()) {
|
|
|
|
// ++iter;
|
|
|
|
// 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.getIdx()] > distance[u->getIdx()] + w) {
|
|
distance[v.getIdx()] = distance[u->getIdx()] + w;
|
|
parent[v.getIdx()] = u;
|
|
Q.push(std::make_pair(&v, distance[v.getIdx()] + acc.getHeuristic(v, *destination))); // SOURCE OR DEST?!
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
|
|
// std::cout << iter << std::endl;
|
|
|
|
// // construct the path
|
|
// std::vector<const T*> path;
|
|
// const T* p = destination;
|
|
// path.push_back(destination);
|
|
|
|
// // until we reached the source-node
|
|
// while (p!=source) {
|
|
// if (p) {
|
|
// p = parent[p->getIdx()];
|
|
// path.push_back(p);
|
|
// } else {
|
|
// return std::vector<const T*>(); //if no path could be found, just return an empty vector.
|
|
// }
|
|
// }
|
|
|
|
// done
|
|
return distance[destination->getIdx()];
|
|
|
|
}
|
|
|
|
|
|
// template <typename Access> static std::vector<const T*> getShortestPathAStar(const T* src, const T* dst, Access acc){
|
|
|
|
// std::vector<const T*> 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<const T*,float>& p1, const std::pair<const T*,float>& p2){
|
|
return p1.second > p2.second;
|
|
}
|
|
};
|
|
|
|
};
|
|
|
|
#endif // ASTAR_H
|