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

added a-star testing version

Browse Source
This commit is contained in:
FrankE
2016-04-13 11:06:00 +02:00
parent e3d245096f
commit e7a5f8eb8c
1 changed files with 21 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -16,16 +16,20 @@ 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) {
// 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();
@@ -35,13 +39,14 @@ public:
// start with infinite distance
for(const auto& node : nodes){
distance[&node] = std::numeric_limits<float>::max();
distance[node.getIdx()] = std::numeric_limits<float>::max();
}
// start at the source
distance[source] = 0.0f;
Q.push(std::make_pair(source,distance[source]));
distance[source->getIdx()] = 0.0f;
Q.push(std::make_pair(source,distance[source->getIdx()]));
int iter = 0;
// proceed until there are now new nodes to follow
while(!Q.empty()) {
@@ -63,15 +68,20 @@ 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, *source)));
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?!
}
}
++iter;
}
std::cout << (std::string) *source << std::endl;
std::cout << (std::string) *destination << std::endl;
std::cout << iter << std::endl;
// construct the path
std::vector<const T*> path;
@@ -81,7 +91,7 @@ public:
// until we reached the source-node
while (p!=source) {
if (p) {
p = parent[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.