124 lines
2.9 KiB
C++
124 lines
2.9 KiB
C++
#ifndef PATH2D_H
|
|
#define PATH2D_H
|
|
|
|
|
|
#include <Indoor/floorplan/v2/Floorplan.h>
|
|
#include "Renderable2D.h"
|
|
#include <Indoor/geo/Point3.h>
|
|
#include <Indoor/nav/dijkstra/DijkstraPath.h>
|
|
#include <Indoor/grid/Grid.h>
|
|
|
|
#include "../nav/grid/Node.h"
|
|
|
|
|
|
class Path2D : public Renderable2D {
|
|
|
|
private:
|
|
|
|
/** the path */
|
|
std::vector<Point3> path;
|
|
|
|
float width = 8;
|
|
QColor color = Qt::blue;
|
|
|
|
public:
|
|
|
|
/** ctor */
|
|
Path2D() {
|
|
|
|
}
|
|
|
|
template <typename Node> void set(const DijkstraPath<Node>& path) {
|
|
|
|
std::vector<Point3> out;
|
|
for (const DijkstraNode<Node>* node : path.getVector()) {
|
|
if (!node) {break;}
|
|
const Node* elem = node->element;
|
|
out.push_back(Point3(elem->x_cm/100.0f, elem->y_cm/100.0f, elem->z_cm/100.0f));
|
|
}
|
|
|
|
set(out);
|
|
|
|
}
|
|
|
|
/** MUST BE CALLED FROM THE MAIN THREAD */
|
|
void set(const std::vector<Point3>& path) {
|
|
this->path = simplify(path);
|
|
}
|
|
|
|
|
|
/** combine nodes while the direction stays the same (many small quads -> one large quad) */
|
|
std::vector<Point3> simplify(const std::vector<Point3>& path) {
|
|
|
|
// copy
|
|
std::vector<Point3> out = path;
|
|
|
|
// remove unneccesary nodes
|
|
for (int i = 1; i < (int) out.size() - 1; ++i) {
|
|
|
|
const Point3 pa = out[i-1];
|
|
const Point3 pb = out[i-0];
|
|
const Point3 pc = out[i+1];
|
|
|
|
// same direction as last segment? combine segments!
|
|
const float dir1 = std::atan2(pb.y-pa.y, pb.x-pa.x); // last edge
|
|
const float dir2 = std::atan2(pc.y-pb.y, pc.x-pb.x); // next edge
|
|
const bool isSameDir = std::abs(dir1-dir2) < 0.03; // last-edge and next-edge have (approx) the same direction?
|
|
if (isSameDir) {out.erase(out.begin()+i); --i; continue;} // no additional information! remove the center node
|
|
|
|
// too many changes in a small space? -> remove some!
|
|
const float d1 = pb.getDistance(pa); // distance to last node
|
|
const float d2 = pb.getDistance(pc); // distance to next node
|
|
const float min = 1.0;
|
|
const bool isPackedChange = d1 < min && d2 < min; // both distances below a threshold?
|
|
if (isPackedChange) {out.erase(out.begin()+i); --i; continue;} // -> many changes in a small area -> remove current node!
|
|
|
|
}
|
|
|
|
return out;
|
|
|
|
}
|
|
|
|
|
|
/** the color to use */
|
|
void setColor(const QColor color) {
|
|
this->color = color;
|
|
}
|
|
|
|
/** the width to use */
|
|
void setWidth(const float w) {
|
|
this->width = w;
|
|
}
|
|
|
|
|
|
protected:
|
|
|
|
void doRender(QPainter& qp, const Scaler2D& s, const RenderParams2D& r) override {
|
|
|
|
QPen pen;
|
|
pen.setWidth(this->width);
|
|
pen.setColor(color);
|
|
qp.setPen(pen);
|
|
|
|
for (int i = 0; i < (int)path.size() - 1; ++i) {
|
|
|
|
const Point3 p1 = path[i];
|
|
const Point3 p2 = path[i+1];
|
|
|
|
if (p1.z < r.clip.belowHeight_m && p2.z < r.clip.belowHeight_m) {continue;}
|
|
if (p1.z > r.clip.aboveHeight_m && p2.z > r.clip.aboveHeight_m) {continue;}
|
|
|
|
const Point2 pa1 = s.mapToScreen(p1.xy());
|
|
const Point2 pa2 = s.mapToScreen(p2.xy());
|
|
|
|
qp.drawLine(pa1.x, pa1.y, pa2.x, pa2.y);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
};
|
|
|
|
#endif // PATH2D_H
|