current revision

ui/map/3D/elements/Path.h

@@ -0,0 +1,220 @@
+#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"
+
+
+class Path : public Renderable {
+
+private:
+
+	GLTriangles<VertNormTex> lines;
+
+	/** path's width (in meter) */
+	float width;
+
+	/** the path's color */
+	QVector4D color;
+
+public:
+
+	/** ctor */
+	Path() {
+		setColor(QVector4D(0.0, 0.4, 1.0, 0.6));
+		setWidth(0.8);
+	}
+
+
+	void initGL() override {
+
+		loadShader(":/res/gl/vertex1.glsl", ":/res/gl/fragmentLine.glsl");
+		//loadShader(":/res/gl/vertex1.glsl", ":/res/gl/fragmentTexSimple.glsl");
+
+		lines.setDiffuse(":/res/gl/tex/arrows.png");
+		program.setUniformValue("texNormalMap", 0);
+
+	}
+
+	/** set the path's color */
+	void setColor(const QVector4D& color) {
+		this->color = color;
+	}
+
+	/** set the path's color */
+	void setColor(const QColor& color) {
+		setColor(QVector4D(color.redF(), color.greenF(), color.blueF(), color.alphaF()));
+	}
+
+	/** set the path's width (in meter) */
+	void setWidth(const float width) {
+		this->width = width;
+	}
+
+	/** render the floor */
+	void _render(const RenderParams& params) override {
+
+		(void) params;
+		program.setUniformValue("color", color);
+
+		//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));
+		}
+
+		set(out);
+
+	}
+
+	/** 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 = 0.8;
+			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;
+
+	}
+
+	/** MUST BE CALLED FROM THE MAIN THREAD */
+	void set(const std::vector<Point3>& _path) {
+
+		std::vector<Point3> path = simplify(_path);
+
+		// reset
+		lines.clear();
+
+		// half the width of the path
+		const float s = this->width * 0.5;
+		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;
+
+		}
+
+		lines.rebuild();
+
+	}
+
+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