add basic collision detection

6 files changed, 128 insertions, 6 deletions

diff --git a/src/drone_controller.cc b/src/drone_controller.cc
index fc1cefe..6ef78ad 100644
--- a/src/drone_controller.cc
+++ b/src/drone_controller.cc
@@ -22,7 +22,8 @@ Waypoint::Waypoint(const QJsonObject &json)
 bool Drone::mesh_initialized = false;
 OpenGLMesh *Drone::mesh = nullptr;
 
-Drone::Drone() {
+Drone::Drone(int id)
+	:id(id) {
 	if (!mesh_initialized) {
 		QVector<GLfloat> verts = load_obj(":/mdl/dji600.obj", LOAD_OBJ_NORMALS | LOAD_OBJ_UVS);
 		QOpenGLTexture *texture = new QOpenGLTexture(QImage(":/img/dji600.jpg").mirrored());
@@ -35,7 +36,7 @@ Drone::Drone() {
 
 
 Drone::Drone(const QJsonObject &json)
-	:Drone() {
+	:Drone(json["id"].toInt()) {
 	QJsonArray ja = json["waypoints"].toArray();
 	waypoints.reserve(ja.size());
 	for (const QJsonValue &o : ja) {
@@ -65,12 +66,23 @@ void Drone::setTo(int frame) {
 	OpenGLMesh &mesh = OpenGLWidget::instance->meshes[mesh_id];
 	mesh.mat = QMatrix4x4();
 	if (next > -1 && prev == -1) {
-		mesh.mat.translate(next_wp->pos);
+		pos = next_wp->pos;
 	} else if (prev > -1 && next == -1) {
-		mesh.mat.translate(prev_wp->pos);
+		pos = prev_wp->pos;
 	} else {
-		mesh.mat.translate(lerp(prev_wp->pos, next_wp->pos, (double) (frame-prev) / (next-prev)));
+		pos = lerp(prev_wp->pos, next_wp->pos, (double) (frame-prev) / (next-prev));
 	}
+	mesh.mat.translate(pos);
+}
+
+
+QVector3D Drone::getPos() const {
+	return pos;
+}
+
+
+int Drone::getId() const {
+	return id;
 }
 
 
@@ -164,3 +176,23 @@ void DroneController::seek(int frame) {
 	this->frame = frame;
 	step();
 }
+
+
+void DroneController::computeCollisions(double sphere_radius) {
+	double sqDist = sphere_radius * sphere_radius * 2;
+	for (int i = 0; i < duration; i++) {
+		for (const Drone &a : drones) {
+			for (const Drone &b : drones) {
+				if (&a == &b) continue;
+				if (collides(a, b, sqDist)) {
+					emit collision(a.getId(), b.getId(), frame);
+				}
+			}
+		}
+	}
+}
+
+
+bool DroneController::collides(const Drone &a, const Drone &b, double sqDist) {
+	return (b.getPos() - a.getPos()).lengthSquared() < sqDist;
+}
diff --git a/src/drone_controller.hh b/src/drone_controller.hh
index 18fc722..93930a1 100644
--- a/src/drone_controller.hh
+++ b/src/drone_controller.hh
@@ -29,12 +29,16 @@ class Drone {
 
 	QVector<Waypoint> waypoints;
 	int mesh_id;
+	QVector3D pos;
+	int id;
 
 public:
-	Drone();
+	Drone(int id);
 	Drone(const QJsonObject &json);
 	const QVector<Waypoint> getWaypoints() const;
 	void setTo(int frame);
+	QVector3D getPos() const;
+	int getId() const;
 };
 
 
@@ -48,6 +52,8 @@ class DroneController : public QObject {
 	QTimer timer;
 	bool paused = true;
 
+	static bool collides(const Drone &a, const Drone &b, double radius);
+
 public:
 	DroneController(const QJsonObject &json);
 	int getDuration() const;
@@ -56,6 +62,7 @@ signals:
 	void frameChanged(int frame);
 	void playing();
 	void pausing();
+	void collision(int idA, int idB, int frame);
 
 private slots:
 	void step();
@@ -66,6 +73,7 @@ public slots:
 	void suspend();
 	void resume();
 	void seek(int frame);
+	void computeCollisions(double sphere_radius);
 };
 
 
diff --git a/src/main_window.cc b/src/main_window.cc
index 7bc7855..e63bf0e 100644
--- a/src/main_window.cc
+++ b/src/main_window.cc
@@ -5,6 +5,7 @@
 #include <QFileDialog>
 #include <QJsonDocument>
 #include <QStyle>
+#include <QDockWidget>
 
 
 MainWindow::MainWindow(QWidget *parent) {
@@ -25,6 +26,12 @@ MainWindow::MainWindow(QWidget *parent) {
 	slider = new QSlider(Qt::Horizontal);
 	bottom_tb.addWidget(slider);
 	slider->setEnabled(false);
+
+	QDockWidget *dock = new QDockWidget("Outils", this);
+	dock->setAllowedAreas(Qt::LeftDockWidgetArea | Qt::RightDockWidgetArea);
+	settings_pane = new SettingsPane();
+	dock->setWidget(settings_pane);
+	addDockWidget(Qt::RightDockWidgetArea, dock);
 }
 
 
@@ -65,6 +72,8 @@ void MainWindow::open(const QString &path) {
 	connect(slider, &QSlider::sliderReleased, dc, &DroneController::resume);
 	connect(slider, &QSlider::valueChanged, dc, &DroneController::seek);
 	connect(dc, &DroneController::frameChanged, slider, &QSlider::setValue);
+	connect(dc, &DroneController::collision, settings_pane, &SettingsPane::addCollision);
+	dc->computeCollisions(10);
 	pause();
 	slider->setEnabled(true);
 }
diff --git a/src/main_window.hh b/src/main_window.hh
index 1e8868a..9bac870 100644
--- a/src/main_window.hh
+++ b/src/main_window.hh
@@ -3,6 +3,7 @@
 
 #include "opengl_widget.hh"
 #include "drone_controller.hh"
+#include "settings_pane.hh"
 
 #include <QMainWindow>
 #include <QTimer>
@@ -20,6 +21,7 @@ class MainWindow : public QMainWindow {
 	QAction *open_action;
 	QAction *playpause_action;
 	QSlider *slider;
+	SettingsPane *settings_pane;
 	DroneController *dc = nullptr;
 	void play();
 	void pause();
diff --git a/src/settings_pane.cc b/src/settings_pane.cc
new file mode 100644
index 0000000..f37c14c
--- /dev/null
+++ b/src/settings_pane.cc
@@ -0,0 +1,44 @@
+#include "settings_pane.hh"
+
+#include <QCheckBox>
+#include <QDoubleSpinBox>
+#include <QFormLayout>
+
+
+SettingsPane::SettingsPane(QWidget *parent)
+	:QWidget(parent) {
+	QDoubleSpinBox *sphere_radius = new QDoubleSpinBox();
+	QCheckBox *show_trajectories = new QCheckBox();
+	QCheckBox *show_support_lines = new QCheckBox();
+	collisions = new QListWidget();
+
+	connect(sphere_radius, QOverload<double>::of(&QDoubleSpinBox::valueChanged),
+			this, &SettingsPane::sphereRadiusChanged);
+	connect(show_trajectories, &QCheckBox::stateChanged,
+			this, &SettingsPane::toggledTrajectories);
+	connect(show_support_lines, &QCheckBox::stateChanged,
+			this, &SettingsPane::toggledSupportLines);
+
+	QFormLayout *layout = new QFormLayout;
+	layout->addRow("Taille de la sphère de collision", sphere_radius);
+	layout->addRow("Afficher les trajectoires", show_trajectories);
+	layout->addRow("Afficher les lignes de support", show_support_lines);
+	layout->addRow(collisions);
+	setLayout(layout);
+}
+
+
+void SettingsPane::addCollision(int idA, int idB, int frame) {
+	QListWidgetItem *item = new QListWidgetItem(QString::number(frame) + ": "
+							   + QString::number(idA) + " / " + QString::number(idB));
+	item->setFlags(Qt::ItemIsEnabled | Qt::ItemNeverHasChildren);
+	collisions->addItem(item);
+}
+
+
+void SettingsPane::clearCollisions() {
+	QListWidgetItem *item;
+	while ((item = collisions->takeItem(0)) != nullptr) {
+		delete item;
+	}
+}
diff --git a/src/settings_pane.hh b/src/settings_pane.hh
new file mode 100644
index 0000000..434f3f7
--- /dev/null
+++ b/src/settings_pane.hh
@@ -0,0 +1,27 @@
+#ifndef SETTINGS_PANE_HH
+#define SETTINGS_PANE_HH
+
+#include <QWidget>
+#include <QListWidget>
+
+
+class SettingsPane : public QWidget {
+	Q_OBJECT
+
+	QListWidget *collisions = nullptr;
+
+public:
+	SettingsPane(QWidget *parent=nullptr);
+
+public slots:
+	void addCollision(int idA, int idB, int frame);
+	void clearCollisions();
+
+signals:
+	void sphereRadiusChanged(double sqRadius);
+	void toggledTrajectories(int shown);
+	void toggledSupportLines(int shown);
+};
+
+
+#endif