From da37a84d5f8b2eeb1c41cba033abebdce12f148a Mon Sep 17 00:00:00 2001
From: ccolin <cyril.colin@etu.univ-amu.fr>
Date: Mon, 4 Jan 2021 01:29:44 +0100
Subject: [PATCH] add drone names when guides are enabled

---
 src/drone_controller.cc | 35 ++++++++++++++++++++++++++---------
 src/opengl_widget.cc    | 18 +++++++++++++++++-
 src/opengl_widget.hh    |  1 +
 3 files changed, 44 insertions(+), 10 deletions(-)

diff --git a/src/drone_controller.cc b/src/drone_controller.cc
index 24cfa2d..99df662 100644
--- a/src/drone_controller.cc
+++ b/src/drone_controller.cc
@@ -5,6 +5,7 @@
 #include <QJsonArray>
 #include <QDebug>
 #include <QOpenGLShaderProgram>
+#include <QPainter>
 
 
 const unsigned char DroneController::sphere_neutral[] = {
@@ -77,7 +78,9 @@ void DroneController::drawTrajectory(QOpenGLExtraFunctions *f, const Drone &d) c
 
 
 void DroneController::drawGuide(QOpenGLExtraFunctions *f, const Drone &d) const {
-	OpenGLWidget::instance->getLineProgram()->bind();
+	OpenGLWidget *glw = OpenGLWidget::instance;
+
+	glw->getLineProgram()->bind();
 	f->glEnableVertexAttribArray(0);
 	f->glBindBuffer(GL_ARRAY_BUFFER, 0);
 	f->glDisable(GL_CULL_FACE);
@@ -87,7 +90,7 @@ void DroneController::drawGuide(QOpenGLExtraFunctions *f, const Drone &d) const
 		d.getPos().x(), d.getPos().y(), d.getPos().z(),
 	};
 	glLineWidth(2);
-	OpenGLWidget::instance->getLineProgram()->setUniformValue("color", .2, .2, .4);
+	glw->getLineProgram()->setUniformValue("color", .2, .2, .4);
 	f->glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, support_line.constData());
 	f->glDrawArrays(GL_LINES, 0, 2);
 
@@ -109,7 +112,7 @@ void DroneController::drawGuide(QOpenGLExtraFunctions *f, const Drone &d) const
 		grid.append(i);
 	}
 	glLineWidth(1);
-	OpenGLWidget::instance->getLineProgram()->setUniformValue("color", .2, .2, .2);
+	glw->getLineProgram()->setUniformValue("color", .2, .2, .2);
 	f->glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, grid.constData());
 	f->glDrawArrays(GL_LINES, 0, grid.size() / 3);
 
@@ -117,23 +120,37 @@ void DroneController::drawGuide(QOpenGLExtraFunctions *f, const Drone &d) const
 	QVector<GLfloat> axes {
 		0, 0, 0, 1, 0, 0,
 		0, 0, 0, 0, 1, 0,
-		0, 0, 0, 0, 0, 1
+		0, 0, 0, 0, 0, 1,
 	};
 	glLineWidth(2);
-	OpenGLWidget::instance->getLineProgram()->setUniformValue("color", 1, 0, 0);
+	glw->getLineProgram()->setUniformValue("color", 1, 0, 0);
 	f->glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, axes.constData());
 	f->glDrawArrays(GL_LINES, 0, 2);
-	OpenGLWidget::instance->getLineProgram()->setUniformValue("color", 0, 1, 0);
+	glw->getLineProgram()->setUniformValue("color", 0, 1, 0);
 	f->glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, axes.constData() + 6);
 	f->glDrawArrays(GL_LINES, 0, 2);
-	OpenGLWidget::instance->getLineProgram()->setUniformValue("color", 0, 0, 1);
+	glw->getLineProgram()->setUniformValue("color", 0, 0, 1);
 	f->glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, axes.constData() + 12);
 	f->glDrawArrays(GL_LINES, 0, 2);
 	glDisable(GL_DEPTH_TEST);
 
 	f->glEnable(GL_CULL_FACE);
-	OpenGLWidget::instance->getLineProgram()->release();
-	OpenGLWidget::instance->getMainProgram()->bind();
+	glw->getLineProgram()->release();
+	glw->getMainProgram()->bind();
+
+	QVector3D text_pos = d.getPos();
+	text_pos.setY(text_pos.y() + .5);
+	QPoint screen_pos;
+	if (!glw->project(text_pos, screen_pos)) return;
+	QPainter painter(glw);
+	painter.endNativePainting();
+	painter.setRenderHint(QPainter::Antialiasing);
+	painter.setPen(Qt::green);
+	QRect rect(0, 0, 200, 100);
+	rect.moveCenter({screen_pos.x(), screen_pos.y()});
+	painter.drawText(rect, Qt::AlignCenter, QString::number(d.getId()));
+	painter.beginNativePainting();
+	painter.end();
 }
 
 
diff --git a/src/opengl_widget.cc b/src/opengl_widget.cc
index 2100af0..a4d9c8c 100644
--- a/src/opengl_widget.cc
+++ b/src/opengl_widget.cc
@@ -176,7 +176,7 @@ void OpenGLWidget::resizeGL(int w, int h) {
 
 
 void OpenGLWidget::paintGL() {
-	glEnable(GL_CULL_FACE); // i shouldn't have to do this every frame, should i?
+	glEnable(GL_CULL_FACE);
 	glEnable(GL_DEPTH_TEST);
 	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 	QMatrix4x4 trans;
@@ -258,3 +258,19 @@ QOpenGLShaderProgram *OpenGLWidget::getMainProgram() {
 QOpenGLShaderProgram *OpenGLWidget::getLineProgram() {
 	return &line_program;
 }
+
+
+bool OpenGLWidget::project(const QVector3D &p, QPoint &point) const {
+	QMatrix4x4 trans;
+	trans.translate(0, 0, -cam_dist);
+	QMatrix4x4 view = trans * rot;
+	QVector3D projected = proj * view * p;
+	if (projected.x() < -1 || projected.x() > 1
+		|| projected.y() < -1 || projected.y() > 1
+		|| projected.z() < -1 || projected.z() > 1) {
+		return false;
+	}
+	point.setX((projected.x() / 2 + .5) * (float) width());
+	point.setY(((projected.y() / 2 - .5) * -1) * (float) height());
+	return true;
+}
diff --git a/src/opengl_widget.hh b/src/opengl_widget.hh
index 04e2ffc..39897be 100644
--- a/src/opengl_widget.hh
+++ b/src/opengl_widget.hh
@@ -58,6 +58,7 @@ public:
 	void setPainter(const Painter *p);
 	QOpenGLShaderProgram *getMainProgram();
 	QOpenGLShaderProgram *getLineProgram();
+	bool project(const QVector3D &p, QPoint &point) const;
 
 signals:
 	void initialized();