1 files changed, 37 insertions, 5 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;
+}