100 lines
3.0 KiB
C++
100 lines
3.0 KiB
C++
/*
|
||
* © Copyright 2014 – Urheberrechtshinweis
|
||
* Alle Rechte vorbehalten / All Rights Reserved
|
||
*
|
||
* Programmcode ist urheberrechtlich geschuetzt.
|
||
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
|
||
* Keine Verwendung ohne explizite Genehmigung.
|
||
* (vgl. § 106 ff UrhG / § 97 UrhG)
|
||
*/
|
||
|
||
#ifndef DIJKSTRAPATH_H
|
||
#define DIJKSTRAPATH_H
|
||
|
||
#include "DijkstraStructs.h"
|
||
#include <vector>
|
||
|
||
#include "../../Assertions.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<const DijkstraNode<T>*> path;
|
||
|
||
public:
|
||
|
||
/** empty ctor */
|
||
DijkstraPath() {
|
||
;
|
||
}
|
||
|
||
/** ctor from end- to start-node */
|
||
DijkstraPath(const DijkstraNode<T>* end, const DijkstraNode<T>* start) {
|
||
|
||
// sanity checks
|
||
Assert::isNotNull(end, "end-node must not be null");
|
||
Assert::isNotNull(start, "start-node must not be null");
|
||
|
||
// follow the path from the end to the start
|
||
const DijkstraNode<T>* curNode = end;
|
||
while (curNode != start) {
|
||
|
||
// sanity check in case no path between start and end exists
|
||
Assert::isNotNull(curNode, "there is not path between start and end. did you accidentially swap start and end?");
|
||
|
||
// append
|
||
path.push_back(curNode);
|
||
curNode = curNode->previous;
|
||
|
||
}
|
||
|
||
}
|
||
|
||
/** allow iteration */
|
||
decltype(path.begin()) begin() {return path.begin();}
|
||
decltype(path.end()) end() {return path.end();}
|
||
// decltype(path.begin()) begin() const {return ((std::vector<const DijkstraNode<T>*>)path).begin();}
|
||
// decltype(path.end()) end() const {return ((std::vector<const DijkstraNode<T>*>)path).end();}
|
||
const DijkstraNode<T>& operator [] (const int idx) const {return *(path[idx]);}
|
||
size_t size() const {return path.size();}
|
||
|
||
/** get the idx-th element from the starting-node. if this is beyond the end, the last node is returned */
|
||
const DijkstraNode<T>& getFromStart(const int idx) const {return (idx >= (int) path.size()) ? (*path.back()) : (*path[idx]);}
|
||
|
||
const std::vector<const DijkstraNode<T>*>& getVector() const {return path;}
|
||
|
||
/** 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
|