319 lines
6.6 KiB
C++
319 lines
6.6 KiB
C++
#ifndef PATH_H
|
|
#define PATH_H
|
|
|
|
#include <Indoor/floorplan/v2/Floorplan.h>
|
|
#include <Indoor/nav/dijkstra/DijkstraPath.h>
|
|
|
|
|
|
#include "../gl/GLHelper.h"
|
|
#include "../gl/GLTriangles.h"
|
|
#include "../Renderable.h"
|
|
|
|
#include "../../lib/gpc/Polygon.h"
|
|
|
|
class Path : public Renderable {
|
|
|
|
private:
|
|
|
|
GLTriangles<VertNormTex> lines;
|
|
|
|
public:
|
|
|
|
/** ctor */
|
|
Path() {
|
|
;
|
|
}
|
|
|
|
|
|
void initGL() override {
|
|
|
|
loadShader(":/res/gl/vertex1.glsl", ":/res/gl/fragmentLine.glsl");
|
|
program.setUniformValue("color", QVector4D(0.0, 0.4, 1.0, 0.6));
|
|
|
|
//loadShader(":/res/gl/vertex1.glsl", ":/res/gl/fragmentTexSimple.glsl");
|
|
|
|
lines.setDiffuse(":/res/gl/tex/arrows.png");
|
|
program.setUniformValue("texNormalMap", 0);
|
|
|
|
}
|
|
|
|
/** render the floor */
|
|
void _render() override {
|
|
lines.rebuild();
|
|
glLineWidth(30);
|
|
glEnable(GL_BLEND);
|
|
glDisable(GL_CULL_FACE);
|
|
glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
|
|
lines.render(&program);
|
|
glDisable(GL_BLEND);
|
|
glEnable(GL_CULL_FACE);
|
|
|
|
}
|
|
|
|
|
|
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));
|
|
}
|
|
|
|
out = simplify(out);
|
|
set(out);
|
|
|
|
}
|
|
|
|
/*
|
|
void setSimple(const std::vector<Point3>& path) {
|
|
|
|
|
|
lines.clear();
|
|
const float s = 0.5;
|
|
|
|
Point3 lastDir(0,0,0);
|
|
std::vector<Floorplan::Quad3> quads;
|
|
|
|
for (int i = 1; i < (int) path.size(); ++i) {
|
|
|
|
Point3 pa = path[i-1];
|
|
Point3 pb = path[i-0];
|
|
const Point3 pc(0, 0, 1);
|
|
|
|
Point3 dir = pb - pa; dir /= dir.length();
|
|
Point3 perb = cross(pa-pb, pc); perb /= perb.length();
|
|
|
|
const Point3 p1 = pa - perb*s;
|
|
const Point3 p2 = pa + perb*s;
|
|
const Point3 p3 = pb + perb*s;
|
|
const Point3 p4 = pb - perb*s;
|
|
|
|
if (dir == lastDir) {
|
|
quads.back().p3 = p3;
|
|
quads.back().p4 = p4;
|
|
} else {
|
|
quads.push_back(Floorplan::Quad3(p1,p2,p3,p4));
|
|
}
|
|
|
|
lastDir = dir;
|
|
|
|
// // produce a small gap between path-lines [will be filled with another quad!]
|
|
// pa += dir * 0.6;
|
|
// pb -= dir * 0.6;
|
|
|
|
//
|
|
}
|
|
|
|
for (int i = 0; i < (int) quads.size(); ++i) {
|
|
|
|
// add the line-segment
|
|
const Floorplan::Quad3 q1 = quads[i];
|
|
addQuad(q1);
|
|
|
|
// // construct the quad between adjacent segments
|
|
// if (i < (int) quads.size() - 1) {
|
|
// const Floorplan::Quad3 q2 = quads[i+1];
|
|
// const Floorplan::Quad3 q3(q1.p3, q2.p2, q2.p1, q1.p4);
|
|
// addQuad(q3);
|
|
// }
|
|
|
|
|
|
}
|
|
|
|
}
|
|
*/
|
|
|
|
/** combine nodes while the direction stays the same (many small quads -> one large quad) */
|
|
std::vector<Point3> simplify(const std::vector<Point3>& path) {
|
|
|
|
std::vector<Point3> out;
|
|
|
|
Point3 lastDir(0,0,0);
|
|
if (!path.empty()) {
|
|
out.push_back(path.back());
|
|
}
|
|
|
|
for (int i = path.size() - 1; i >= 1; --i) {
|
|
|
|
const Point3 pa = path[i-0];
|
|
const Point3 pb = path[i-1];
|
|
const Point3 dir = (pb - pa).normalized();
|
|
|
|
if (dir == lastDir) {
|
|
out[out.size()-1] = pb;
|
|
} else {
|
|
out.push_back(pb);
|
|
}
|
|
|
|
lastDir = dir;
|
|
|
|
}
|
|
|
|
// 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];
|
|
|
|
const float min = 0.6;
|
|
const float d1 = pb.getDistance(pa);
|
|
const float d2 = pb.getDistance(pc);
|
|
|
|
if (d1 < min || d2 < min) {
|
|
out.erase(out.begin() + i);
|
|
}
|
|
|
|
}
|
|
|
|
return out;
|
|
|
|
}
|
|
|
|
// void set(const std::vector<Point3>& path) {
|
|
|
|
// lines.clear();
|
|
|
|
// const float s = 0.4;
|
|
// std::vector<Point3> pts;
|
|
|
|
// for (int i = 0; i < (int) path.size(); ++i) {
|
|
|
|
// const Point3 pa = path[i-1];
|
|
// const Point3 pb = path[i-0];
|
|
// const Point3 pc(0, 0, 1);
|
|
|
|
// const Point3 perb = cross(pa-pb, pc).normalized();
|
|
|
|
// // quad's edges
|
|
// const Point3 p1 = pa - perb*s;
|
|
// const Point3 p2 = pa + perb*s;
|
|
// const Point3 p3 = pb + perb*s;
|
|
// const Point3 p4 = pb - perb*s;
|
|
|
|
// pts.push_back(p1);
|
|
// pts.push_back(p2);
|
|
|
|
// }
|
|
|
|
// std::vector<Floorplan::Quad3> quads;
|
|
// for (int i = 0; i < (int) pts.size(); i+=2) {
|
|
// quads.push_back(Floorplan::Quad3(pts[i+0], pts[i+1], pts[i+3], pts[i+2]));
|
|
// }
|
|
|
|
// float l1 = 0;
|
|
// float l2 = 0;
|
|
|
|
// for (int i = 0; i < (int) quads.size(); ++i) {
|
|
|
|
// // add the line-segment
|
|
// const Floorplan::Quad3 q1 = quads[i];
|
|
// l2 += ((q1.p1 + q1.p2) / 2).getDistance( (q1.p3 + q1.p4) / 2 );
|
|
// addQuad(q1, l1, l2);
|
|
// l1 = l2;
|
|
|
|
// }
|
|
|
|
// }
|
|
|
|
|
|
|
|
void set(const std::vector<Point3>& path) {
|
|
|
|
|
|
lines.clear();
|
|
|
|
// half the width of the path
|
|
const float s = 0.4;
|
|
std::vector<Floorplan::Quad3> quads;
|
|
|
|
for (int i = 1; i < (int) path.size(); ++i) {
|
|
|
|
Point3 pa = path[i-1];
|
|
Point3 pb = path[i-0];
|
|
const Point3 pc(0, 0, 1);
|
|
Point3 dir = (pb - pa).normalized();
|
|
|
|
// produce a small gap between path-segments
|
|
// those segments will be smoothly connected using another quad
|
|
pa += dir * 0.35;
|
|
pb -= dir * 0.35;
|
|
|
|
const Point3 perb = cross(pa-pb, pc).normalized();
|
|
|
|
// quad's edges
|
|
const Point3 p1 = pa - perb*s;
|
|
const Point3 p2 = pa + perb*s;
|
|
const Point3 p3 = pb + perb*s;
|
|
const Point3 p4 = pb - perb*s;
|
|
|
|
// add
|
|
quads.push_back(Floorplan::Quad3(p1,p2,p3,p4));
|
|
|
|
}
|
|
|
|
float l1 = 0;
|
|
float l2 = 0;
|
|
|
|
for (int i = 0; i < (int) quads.size(); ++i) {
|
|
|
|
// add the line-segment
|
|
const Floorplan::Quad3 q1 = quads[i];
|
|
l2 += ((q1.p1 + q1.p2) / 2).getDistance( (q1.p3 + q1.p4) / 2 );
|
|
addQuad(q1, l1, l2);
|
|
l1 = l2;
|
|
|
|
// done?
|
|
if (i == (int) quads.size() - 1) {break;}
|
|
|
|
// construct the quad between adjacent segments
|
|
const Floorplan::Quad3 q2 = quads[i+1];
|
|
const Floorplan::Quad3 q3(q1.p4, q1.p3, q2.p2, q2.p1);
|
|
l2 += ((q3.p1 + q3.p2) / 2).getDistance( (q3.p3 + q3.p4) / 2 );
|
|
addQuad(q3, l1, l2);
|
|
l1 = l2;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
private:
|
|
|
|
void addQuad(const Floorplan::Quad3& q, const float l1, const float l2) {
|
|
|
|
// move the path upwards (slightly above the ground)
|
|
const float h = 0.40;
|
|
|
|
const QVector3D v1(q.p1.x, q.p1.z+h, q.p1.y);
|
|
const QVector3D v2(q.p2.x, q.p2.z+h, q.p2.y);
|
|
const QVector3D v3(q.p3.x, q.p3.z+h, q.p3.y);
|
|
const QVector3D v4(q.p4.x, q.p4.z+h, q.p4.y);
|
|
|
|
const QVector3D n(0,1,0);
|
|
|
|
const QVector2D tex1(0, l1);
|
|
const QVector2D tex2(1, l1);
|
|
const QVector2D tex3(1, l2);
|
|
const QVector2D tex4(0, l2);
|
|
|
|
// const QVector2D tex1(q.p1.x, q.p1.y);
|
|
// const QVector2D tex2(q.p2.x, q.p2.y);
|
|
// const QVector2D tex3(q.p3.x, q.p3.y);
|
|
// const QVector2D tex4(q.p4.x, q.p4.y);
|
|
|
|
const VertNormTex vnt1(v1, n, tex1);
|
|
const VertNormTex vnt2(v2, n, tex2);
|
|
const VertNormTex vnt3(v3, n, tex3);
|
|
const VertNormTex vnt4(v4, n, tex4);
|
|
|
|
|
|
lines.addQuadCCW(vnt1, vnt2, vnt3, vnt4);
|
|
|
|
}
|
|
|
|
|
|
};
|
|
|
|
#endif // PATH_H
|