66 lines
1.9 KiB
C++
66 lines
1.9 KiB
C++
#ifndef DEBUGSHORTESTPATH_H
|
|
#define DEBUGSHORTESTPATH_H
|
|
|
|
#include <Indoor/grid/walk/GridWalkShortestPathControl.h>
|
|
|
|
#include <KLib/misc/gnuplot/Gnuplot.h>
|
|
#include <KLib/misc/gnuplot/GnuplotSplot.h>
|
|
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
|
|
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
|
|
|
|
#include "../Helper.h"
|
|
#include "../Vis.h"
|
|
|
|
template <typename T> class DebugShortestPath : public GridWalkShortestPathControl<T> {
|
|
|
|
private:
|
|
|
|
Vis vis;
|
|
|
|
|
|
public:
|
|
|
|
/** ctor */
|
|
template <typename Access> DebugShortestPath(Grid<T>& grid, const Access& acc, const T& target, Helper::FHWSFloors& floors) : GridWalkShortestPathControl<T>(grid, acc, target) {
|
|
|
|
vis.particles.setColorHex("#0000ff");
|
|
vis.particles.setPointSize(1.5);
|
|
|
|
vis.addFloor(floors.f0, floors.h0);
|
|
vis.addFloor(floors.f1, floors.h1);
|
|
vis.addFloor(floors.f2, floors.h2);
|
|
vis.addFloor(floors.f3, floors.h3);
|
|
|
|
}
|
|
|
|
GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad) {
|
|
|
|
GridWalkState<T> s = GridWalkShortestPathControl<T>::getDestination(grid, start, distance_m, headChange_rad);
|
|
|
|
if (this->recalc == 0){
|
|
vis.estPath.clear();
|
|
vis.particles.clear();
|
|
vis.particles.add(K::GnuplotPoint3(this->centerOfMass.x, this->centerOfMass.y, this->centerOfMass.z));
|
|
const int advance = this->stdDevDist / grid.getGridSize_cm();
|
|
const T& d = *this->path->getFromStart(advance).element;
|
|
vis.particles.add(K::GnuplotPoint3(d.x_cm, d.y_cm, d.z_cm));
|
|
for (int i = 0; i < (int)this->path->size()-1; ++i) {
|
|
const DijkstraNode<T>& dn1 = (*this->path)[i+0];
|
|
const DijkstraNode<T>& dn2 = (*this->path)[i+1];
|
|
K::GnuplotPoint3 p1 (dn1.element->x_cm, dn1.element->y_cm, dn1.element->z_cm);
|
|
K::GnuplotPoint3 p2 (dn2.element->x_cm, dn2.element->y_cm, dn2.element->z_cm);
|
|
vis.estPath.addSegment(p1, p2);
|
|
}
|
|
vis.show();
|
|
|
|
}
|
|
|
|
|
|
return s;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // DEBUGSHORTESTPATH_H
|