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

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

@@ -16,41 +16,47 @@ template <typename T> class AStar {
public:
#define LE_MAX 500000
//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
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
std::unordered_map<const T*, const T*> parent;
//std::unordered_map<const T*, const T*> parent;
const T* parent[LE_MAX];
// 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
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] = std::numeric_limits<float>::max();
}
// 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] = 0.0f;
Q.push(std::make_pair(source,distance[source]));
int iter = 0;
//std::cout << (std::string)*source << std::endl;
//std::cout << (std::string)*destination << std::endl;
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;
// ++iter;
// fetch the next-nearest node from the queue
const T* u = Q.top().first;
@@ -69,35 +75,35 @@ public:
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, *destination)));
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;
// std::cout << iter << std::endl;
// construct the path
std::vector<const T*> path;
const T* p = destination;
path.push_back(destination);
// // 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];
path.push_back(p);
} else {
return std::vector<const T*>(); //if no path could be found, just return an empty vector.
}
}
// // 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 path;
return distance[destination->getIdx()];
}