kazu
9947dced15
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...
59 lines
1.4 KiB
C++
59 lines
1.4 KiB
C++
#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
|