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

added several grid-walks

Browse Source 
added new helper methods/classes (e.g. for heading)
new test cases
optimize the dijkstra
cleanups/refactoring
added timed-benchmarks to the log
many more...
This commit is contained in:
FrankE
2016-01-24 18:59:06 +01:00
parent cdf97322f8
commit 9947dced15
30 changed files with 1406 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files

62
nav/dijkstra/Dijkstra.h
View File

@@ -5,9 +5,11 @@
#include <vector>
#include <algorithm>
#include <unordered_set>
#include <list>
#include "DijkstraStructs.h"
#include "../../misc/Debug.h"
#include "../../misc/Time.h"
#include <KLib/Assertions.h>
@@ -19,26 +21,11 @@ template <typename T> class Dijkstra {
/** all already processed edges */
std::unordered_set<DijkstraEdge<T>> usedEdges;
/** to-be-processed nodes (USE LINKED LIST INSTEAD?!) */
/** to-be-processed nodes (NOTE: using std::list here was SLOWER!) */
std::vector<DijkstraNode<T>*> toBeProcessedNodes;
public:
/** get (or create) a new node for the given user-node */
DijkstraNode<T>* getNode(const T* userNode) {
if (nodes.find(userNode) == nodes.end()) {
DijkstraNode<T>* dn = new DijkstraNode<T>(userNode);
nodes[userNode] = dn;
}
return nodes[userNode];
}
/** get the edge (bi-directional) between the two given nodes */
DijkstraEdge<T> getEdge(const DijkstraNode<T>* n1, const DijkstraNode<T>* n2) {
return DijkstraEdge<T>(n1, n2);
}
/** get the dijkstra-pendant for the given user-node */
DijkstraNode<T>* getNode(const T& userNode) {
return nodes[&userNode];
@@ -50,7 +37,10 @@ public:
// NOTE: end is currently ignored!
// runs until all nodes were evaluated
Log::add("Dijkstra", "calculating dijkstra from " + (std::string)start);
// compare two nodes by their distance from the start
static auto comp = [] (const DijkstraNode<T>* n1, const DijkstraNode<T>* n2) {return n1->cumWeight < n2->cumWeight;};
Log::add("Dijkstra", "calculating dijkstra from " + (std::string)start + " to ALL OTHER nodes");
// cleanup
toBeProcessedNodes.clear();
@@ -71,10 +61,14 @@ public:
while(!toBeProcessedNodes.empty()) {
// get the next to-be-processed node
DijkstraNode<T>* dnSrc = toBeProcessedNodes[0];
const auto min = std::min_element(toBeProcessedNodes.begin(), toBeProcessedNodes.end(), comp);
DijkstraNode<T>* dnSrc = *min;
// stop when end was reached??
//if (dnSrc->element == &end) {break;}
// and remove him from the list
toBeProcessedNodes.erase(toBeProcessedNodes.begin());
toBeProcessedNodes.erase(min);
// process each neighbor of the current element
for (int i = 0; i < acc.getNumNeighbors(*dnSrc->element); ++i) {
@@ -90,7 +84,7 @@ public:
DijkstraNode<T>* dnDst = getNode(dst);
// get-or-create the edge describing the connection
DijkstraEdge<T> edge = getEdge(dnSrc, dnDst);
const DijkstraEdge<T> edge = getEdge(dnSrc, dnDst);
// was this edge already processed? -> skip it
if (usedEdges.find(edge) != usedEdges.end()) {continue;}
@@ -98,6 +92,8 @@ public:
// otherwise: remember it
usedEdges.insert(edge);
// and add the node for later processing
toBeProcessedNodes.push_back(dnDst);
@@ -110,18 +106,30 @@ public:
}
// sort the nodes by distance-from-start (shortest first)
auto comp = [] (const DijkstraNode<T>* n1, const DijkstraNode<T>* n2) {return n1->cumWeight < n2->cumWeight;};
std::sort(toBeProcessedNodes.begin(), toBeProcessedNodes.end(), comp);
}
Log::add("Dijkstra", "processed " + std::to_string(nodes.size()) + " nodes");
// cleanup
// reclaim temporal memory
toBeProcessedNodes.clear();
usedEdges.clear();
Log::add("Dijkstra", "processed " + std::to_string(nodes.size()) + " nodes");
}
private:
/** get (or create) a new node for the given user-node */
inline DijkstraNode<T>* getNode(const T* userNode) {
if (nodes.find(userNode) == nodes.end()) {
DijkstraNode<T>* dn = new DijkstraNode<T>(userNode);
nodes[userNode] = dn;
}
return nodes[userNode];
}
/** get the edge (bi-directional) between the two given nodes */
inline DijkstraEdge<T> getEdge(const DijkstraNode<T>* n1, const DijkstraNode<T>* n2) const {
return DijkstraEdge<T>(n1, n2);
}
};

58
nav/dijkstra/DijkstraPath.h Normal file
View File

@@ -0,0 +1,58 @@
#ifndef DIJKSTRAPATH_H
#define DIJKSTRAPATH_H
#include "DijkstraStructs.h"
/**
* describes a dijkstra-generated path between end and start.
* allows KNN searches for points within this path.
*
*/
template <typename T> class DijkstraPath {
private:
/** the constructed path */
std::vector<DijkstraNode<T>*> path;
public:
/** ctor from end- to start-node */
DijkstraPath(DijkstraNode<T>* end, DijkstraNode<T>* start) {
// follow the path from the end to the start
DijkstraNode<T>* curNode = end;
while (curNode != start) {
path.push_back(curNode);
curNode = curNode->previous;
}
}
/** NANOFLANN: number of elements in the path */
inline int kdtree_get_point_count() const {
return path.size();
}
/** NANOFLANN: use default bbox */
template <class BBOX> inline bool kdtree_get_bbox(BBOX& bb) const {
(void) bb; return false;
}
/** NANOFLANN: get the idx-th elements dim-th dimension */
inline float kdtree_get_pt(const size_t idx, const int dim) const {
return (*(path[idx]->element))[dim];
}
/** NANOFLANN: get the distance between the given point and the idx-th element */
inline float kdtree_distance(const float* p1, const size_t idx_p2, size_t) const {
const DijkstraNode<T>* n = path[idx_p2];
const float d0 = p1[0] - (*(n->element))[0];
const float d1 = p1[1] - (*(n->element))[1];
const float d2 = p1[2] - (*(n->element))[2];
return (d0*d0) + (d1*d1) + (d2*d2);
}
};
#endif // DIJKSTRAPATH_H

27
nav/dijkstra/DijkstraStructs.h
View File

@@ -20,23 +20,20 @@ template <typename T> struct DijkstraNode {
float cumWeight;
// /** ctor */
// DijkstraNode() : element(nullptr), previous(), cumWeight(INF) {;}
/** ctor */
DijkstraNode(const T* element) : element(element), previous(), cumWeight(INF) {;}
/** equal? (bi-dir) */
bool operator == (const DijkstraNode<T>& other) {
return element == other.element;
}
// /** equal? (bi-dir) */
// bool operator == (const DijkstraNode<T>& other) const {
// return element == other.element;
// }
};
/**
* data structure describing the connection between two nodes
* only used to track already processed connections!
* data structure describing the connection between two nodes.
* NOTE: only used to track already processed connections!
*/
template <typename T> struct DijkstraEdge {
@@ -49,7 +46,7 @@ template <typename T> struct DijkstraEdge {
/** ctor */
DijkstraEdge(const DijkstraNode<T>* src, const DijkstraNode<T>* dst) : src(src), dst(dst) {;}
/** equal? (bi-dir) */
/** equal? (bi-directional! edge direction does NOT matter) */
bool operator == (const DijkstraEdge& other) const {
return ((dst == other.dst) && (src == other.src)) ||
((src == other.dst) && (dst == other.src));
@@ -57,15 +54,7 @@ template <typename T> struct DijkstraEdge {
};
//template <typename T> struct DijkstraEdgeWeighted : public DijkstraEdge<T> {
// /** the edge's weight */
// float weight;
// DijkstraEdgeWeighted(const DijkstraNode<T>* src, const DijkstraNode<T>* dst, const float weight) : DijkstraEdge<T>(src,dst), weight(weight) {;}
//};
/** allows adding DijkstraEdge<T> to hash-maps */
namespace std {
template <typename T> struct hash<DijkstraEdge<T>>{
size_t operator()(const DijkstraEdge<T>& e) const {