FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

edit astar

Browse Source
This commit is contained in:
toni
2016-04-14 13:04:16 +02:00
parent c0ab98005a e7a5f8eb8c
commit a00f2cf80a
1 changed files with 40 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

72
nav/a-star/AStar.h
View File

@@ -16,41 +16,47 @@ template <typename T> class AStar {
public: public:
#define LE_MAX 500000
//dijkstra with priority queue O(E log V) //dijkstra with priority queue O(E log V)
template <typename Access> static std::vector<const T*> get(const T* source, const T* destination, Access acc) { template <typename Access>
static float get(const T* source, const T* destination, Access acc) {
// track distances from the source to each other node // track distances from the source to each other node
std::unordered_map<const T*, float> distance; //std::unordered_map<const T*, float> distance;
float distance[LE_MAX];
// track the previous node for each node along the path // track the previous node for each node along the path
std::unordered_map<const T*, const T*> parent; //std::unordered_map<const T*, const T*> parent;
const T* parent[LE_MAX];
// all nodes // all nodes
const std::vector<T>& nodes = acc.getAllNodes(); //const std::vector<T>& nodes = acc.getAllNodes();
// priority queue to check which node is to-be-processed next // 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; std::priority_queue<std::pair<T*,float>, std::vector<std::pair<const T*,float>>, Comparator2> Q;
// start with infinite distance // start with infinite distance
for(const auto& node : nodes){ // for(const auto& node : nodes){
distance[&node] = std::numeric_limits<float>::max(); // distance[node.getIdx()] = std::numeric_limits<float>::max();
} // }
std::fill_n(distance, LE_MAX, std::numeric_limits<float>::max());
// start at the source
distance[source] = 0.0f;
Q.push(std::make_pair(source,distance[source]));
int iter = 0;
// std::cout << (std::string)*source << std::endl; // std::cout << (std::string)*source << std::endl;
// std::cout << (std::string)*destination << 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 // proceed until there are now new nodes to follow
while(!Q.empty()) { while(!Q.empty()) {
++iter; // ++iter;
// fetch the next-nearest node from the queue // fetch the next-nearest node from the queue
const T* u = Q.top().first; const T* u = Q.top().first;
@@ -69,35 +75,35 @@ public:
const float w = acc.getWeightBetween(v, *u); const float w = acc.getWeightBetween(v, *u);
// found a better route? // found a better route?
if (distance[&v] > distance[u] + w) { if (distance[v.getIdx()] > distance[u->getIdx()] + w) {
distance[&v] = distance[u] + w; distance[v.getIdx()] = distance[u->getIdx()] + w;
parent[&v] = u; parent[v.getIdx()] = u;
Q.push(std::make_pair(&v, distance[&v] + acc.getHeuristic(v, *destination))); Q.push(std::make_pair(&v, distance[v.getIdx()] + acc.getHeuristic(v, *destination))); // SOURCE OR DEST?!
} }
} }
} }
// std::cout << iter << std::endl; // std::cout << iter << std::endl;
// // construct the path
// std::vector<const T*> path;
// const T* p = destination;
// path.push_back(destination);
// construct the path // // until we reached the source-node
std::vector<const T*> path; // while (p!=source) {
const T* p = destination; // if (p) {
path.push_back(destination); // p = parent[p->getIdx()];
// path.push_back(p);
// until we reached the source-node // } else {
while (p!=source) { // return std::vector<const T*>(); //if no path could be found, just return an empty vector.
if (p) { // }
p = parent[p]; // }
path.push_back(p);
} else {
return std::vector<const T*>(); //if no path could be found, just return an empty vector.
}
}
// done // done
return path; return distance[destination->getIdx()];
} }