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: 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 std::vector<const T*> 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();
@@ -35,13 +39,14 @@ public:
// 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();
} }
// start at the source // start at the source
distance[source] = 0.0f; distance[source->getIdx()] = 0.0f;
Q.push(std::make_pair(source,distance[source])); Q.push(std::make_pair(source,distance[source->getIdx()]));
int iter = 0;
// proceed until there are now new nodes to follow // proceed until there are now new nodes to follow
while(!Q.empty()) { while(!Q.empty()) {
@@ -63,15 +68,20 @@ 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, *source))); 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 // construct the path
std::vector<const T*> path; std::vector<const T*> path;
@@ -81,7 +91,7 @@ public:
// until we reached the source-node // until we reached the source-node
while (p!=source) { while (p!=source) {
if (p) { if (p) {
p = parent[p]; p = parent[p->getIdx()];
path.push_back(p); path.push_back(p);
} else { } else {
return std::vector<const T*>(); //if no path could be found, just return an empty vector. return std::vector<const T*>(); //if no path could be found, just return an empty vector.