add option to draw trajectories

1 files changed, 41 insertions, 0 deletions

diff --git a/src/drone_controller.cc b/src/drone_controller.cc
index 512ad23..dbe9a00 100644
--- a/src/drone_controller.cc
+++ b/src/drone_controller.cc
@@ -44,6 +44,38 @@ DroneController::DroneController(const QJsonObject &json)
 }
 
 
+void DroneController::drawTrajectory(QOpenGLExtraFunctions *f, const Drone &d) const {
+	OpenGLWidget::instance->getLineProgram()->bind();
+	OpenGLWidget::instance->getLineProgram()->setUniformValue("color", 1, 0, .532);
+	size_t trajectory_len = 1;
+	for (const Waypoint &wp : d.getWaypoints()) {
+		if (wp.frame > frame) break;
+		trajectory_len++;
+	}
+	GLfloat trajectory[trajectory_len * 3] = {0};
+	size_t i = 0;
+	for (const Waypoint &wp : d.getWaypoints()) {
+		if (wp.frame > frame) break;
+		trajectory[i] = wp.pos.x();
+		trajectory[i + 1] = wp.pos.y();
+		trajectory[i + 2] = wp.pos.z();
+		i += 3;
+	}
+	trajectory[i] = d.getPos().x();
+	trajectory[i + 1] = d.getPos().y();
+	trajectory[i + 2] = d.getPos().z();
+	f->glEnableVertexAttribArray(0);
+	f->glBindBuffer(GL_ARRAY_BUFFER, 0);
+	f->glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, trajectory);
+	glLineWidth(2);
+	f->glDisable(GL_CULL_FACE);
+	f->glDrawArrays(GL_LINE_STRIP, 0, trajectory_len);
+	f->glEnable(GL_CULL_FACE);
+	OpenGLWidget::instance->getLineProgram()->release();
+	OpenGLWidget::instance->getMainProgram()->bind();
+}
+
+
 void DroneController::draw(QOpenGLExtraFunctions *f) const {
 	const QVector<QPair<int, int>> &col = collisions[frame];
 	for (const Drone &d : drones) {
@@ -60,6 +92,9 @@ void DroneController::draw(QOpenGLExtraFunctions *f) const {
 			sphere->draw(f, mat);
 		}
 		OpenGLWidget::instance->getMainProgram()->setUniformValue("highlight", false);
+		if (draw_trajectories) {
+			drawTrajectory(f, d);
+		}
 	}
 }
 
@@ -155,3 +190,9 @@ void DroneController::displaySpheres(double sphere_radius) {
 bool DroneController::collides(const Drone &a, const Drone &b, double sqDist) {
 	return (b.getPos() - a.getPos()).lengthSquared() < sqDist;
 }
+
+
+void DroneController::setDrawTrajectories(bool enable) {
+	draw_trajectories = enable;
+	OpenGLWidget::instance->update();
+}