use vector for choose direction

CMakeLists.txt

@@ -4,7 +4,7 @@ project(ProtocolDeveloppement)
 set(CMAKE_CXX_STANDARD 14)
 
 #set(CMAKE_PREFIX_PATH "/Qt/6.5.0/android_arm64_v8a/lib/cmake")
-set(CMAKE_PREFIX_PATH "D:/programme/Qt/6.5.0/mingw_64/lib/cmake")
+#set(CMAKE_PREFIX_PATH "D:/programme/Qt/6.5.0/mingw_64/lib/cmake")
 
 
 find_package(Qt6Widgets REQUIRED)
@@ -16,6 +16,7 @@ add_executable(${PROJECT_NAME}
         stompframe.cpp
         stomp.cpp
         app.cpp
+        Vector2D.cpp
 )
 
 link_directories( ${PCL_LIBRARY_DIRS} )

Vector2D.cpp

@@ -0,0 +1,49 @@
+#include "Vector2D.h"
+
+Vector2D::Vector2D(Point point) {
+    point_.x = point.x;
+    point_.y = point.y;
+    lenght_ = sqrt(pow(point.x, 2) + pow(point.y, 2));
+    angle_ = atan2(point.y, point.x);
+}
+
+Vector2D::Vector2D(double lenght, double angle) {
+    lenght_ = lenght;
+    angle_ = angle;
+    point_.x = lenght * cos(angle);
+    point_.y = lenght * sin(angle);
+}
+
+Vector2D::Point Vector2D::CreatePoint(double x, double y) {
+    Point p;
+    p.x = x;
+    p.y = y;
+    return p;
+}
+
+void Vector2D::normalize(double max) {
+    lenght_ = (1/lenght_);
+    point_.x = lenght_ * cos(angle_);
+    point_.y = lenght_ * sin(angle_);
+}
+
+Vector2D Vector2D::reverse() {
+    Point p;
+    p.x = -point_.x;
+    p.y = -point_.y;
+    return Vector2D(p);
+}
+
+Vector2D Vector2D::operator +(Vector2D v) const {
+    Point p;
+    p.x = point_.x + v.x();
+    p.y = point_.y + v.y();
+    return Vector2D(p);
+}
+
+Vector2D Vector2D::operator -(Vector2D v) const {
+    Point p;
+    p.x = point_.x - v.x();
+    p.y = point_.y - v.y();
+    return Vector2D(p);
+}

Vector2D.h

@@ -0,0 +1,41 @@
+#ifndef VECTOR2D_H
+#define VECTOR2D_H
+
+#include <cmath>
+
+class Vector2D {
+
+public:
+    typedef struct {
+        double x;
+        double y;
+    } Point;
+    Vector2D() = default;
+    Vector2D(Point point);
+    Vector2D(double lenght, double angle);
+    ~Vector2D() = default;
+
+    static Point CreatePoint(double x, double y);
+
+    double x() {return point_.x;}
+    double y() {return point_.y;}
+    Point point() {return point_;}
+    double lenght() {return lenght_;}
+    double angle() {return angle_;}
+
+    void normalize(double max);
+    Vector2D reverse();
+
+    Vector2D operator +(Vector2D v) const;
+    Vector2D operator -(Vector2D v) const;
+
+private:
+    Point point_;
+    double lenght_;
+    double angle_;
+
+};
+
+
+
+#endif //VECTOR2D_H

app.cpp

@@ -1,9 +1,9 @@
 #include "app.h"
 
+
 App::App(Stomp* st) {
     st_ = st;
     st_->connectRequest("sdi.hevs.ch", 61614, "/", "sdi10", "809c02f36becb0868da98761fe3209f6");
-    st_->sendRequest("/topic/sdi10.gem.command", "right");
     st_->subscribeRequest("/topic/sdi10.gem.field", 1);
 
 }
@@ -38,19 +38,11 @@ void App::disconnectIndication() {
 
 void App::addGem(int x, int y, int pts) {
     Gem g;
-    g.x = x;
-    g.y = y;
+    g.coordinate = Vector2D(Vector2D::CreatePoint(x, GRID_SIZE-y));
     g.pts = pts;
-    g.relativePts = computeRelativePts(g);
     gems_.append(g);
 }
 
-void App::printGem() {
-    for (Gem g: gems_) {
-        qDebug() << g.x << ":" << g.y << " - " << g.pts << Qt::endl;
-    }
-}
-
 void App::fillField(QString body) {
     int x = 0;
     int y = 0;
@@ -58,9 +50,7 @@ void App::fillField(QString body) {
     for(int i = 0; i<body.length(); i++) {
         const QChar c = body.at(i);
         if(c == 'Y') {
-            myVehicle_.x = x;
-            myVehicle_.y = y;
-            myVehicle_.me = true;
+            myVehicle_ = Vector2D(Vector2D::CreatePoint(x, GRID_SIZE-y));
         }
         if(c == 'g') addGem(x, y, 100);
         if(c == 'G') addGem(x, y, 250);
@@ -74,18 +64,65 @@ void App::fillField(QString body) {
     }
 }
 
-QVector2D App::computeRelativePts(Gem g) {
-    QVector2D v = QVector2D(myVehicle_.x, myVehicle_.y) - QVector2D(g.x, g.y);
+Vector2D App::computeRelativePts(Gem g) {
+    Vector2D v = g.coordinate-myVehicle_;
+    v.reverse();
+    v.normalize(MAX_LENGHT);
+
+    return Vector2D(v.lenght()*g.pts*v.lenght(), v.angle());
 }
 
 void App::computeMove() {
-    Gem myGem;
+    static const double PI = 3.14159265358979323846;
+    Vector2D myRelativePts;
     for(Gem g : gems_) {
-        if(g.pts > myGem.pts) myGem = g;
+        myRelativePts = myRelativePts+computeRelativePts(g);
+    }
+    double angle = myRelativePts.angle();
+
+    if(angle <0) angle = angle+2*PI;
+    angle *= 180/PI;
+
+    qDebug() << "Angle: " << angle << "°";
+    qDebug() << "Length: " << myRelativePts.lenght() << Qt::endl;
+
+    if(angle > 360) {
+        qDebug() << "Error angle: " << angle << Qt::endl;
+    }
+    else if(angle <= -360) {
+        qDebug() << "Error angle: " << angle << Qt::endl;
+    }
+
+    else if(angle > 315) { // angle > 315°
+        st_->sendRequest("/topic/sdi10.gem.command", "right");
+    }
+    else if(angle <= -315) { // angle <= -315° || angle <= 45°
+        st_->sendRequest("/topic/sdi10.gem.command", "right");
+    }
+
+    else if(angle > 225) { // angle > 225°
+        st_->sendRequest("/topic/sdi10.gem.command", "down");
+    }
+    else if(angle <= -225) { // angle <= -225° || angle <= 135°
+        st_->sendRequest("/topic/sdi10.gem.command", "up");
+    }
+
+    else if(angle > 135) { // angle > 135°
+        st_->sendRequest("/topic/sdi10.gem.command", "left");
+    }
+    else if(angle <= -135) { // angle <= -135° || angle <= 225°
+        st_->sendRequest("/topic/sdi10.gem.command", "left");
+    }
+
+    else if(angle > 45) { // angle > 45°
+        st_->sendRequest("/topic/sdi10.gem.command", "up");
+    }
+    else if(angle <= -45) { // angle <= -45° || angle <= 315°
+        st_->sendRequest("/topic/sdi10.gem.command", "down");
+    }
+
+    else {
+        st_->sendRequest("/topic/sdi10.gem.command", "right");
     }
-    if(myGem.x>myVehicle_.x) st_->sendRequest("/topic/sdi10.gem.command", "right");
-    else if(myGem.x<myVehicle_.x) st_->sendRequest("/topic/sdi10.gem.command", "left");
-    else if(myGem.y>myVehicle_.y) st_->sendRequest("/topic/sdi10.gem.command", "down");
-    else if(myGem.y<myVehicle_.y) st_->sendRequest("/topic/sdi10.gem.command", "up");
 }
 

app.h

@@ -5,12 +5,19 @@
 #include "interface/iStompObserver.h"
 #include "stomp.h"
 #include <QVector2D>
+#include <cmath>
+
+#include "Vector2D.h"
+
+#define GRID_SIZE 32
+
 
 class App : public QObject, public interface::iStompObserver {
 public:
     App(Stomp* st);
     ~App() = default;
 
+    const double MAX_LENGHT = sqrt(GRID_SIZE*GRID_SIZE + GRID_SIZE*GRID_SIZE);
     // iStompObserver interface
 private:
     void connectConfirmation(bool success, QString version);
@@ -20,14 +27,12 @@ private:
     void disconnectConfirmation();
     void disconnectIndication();
 
-private:
+protected:
     Stomp* st_;
 
     typedef struct {
-        int x;
-        int y;
+        Vector2D coordinate;
         int pts;
-        QVector2D relativePts;
     } Gem;
 
     typedef struct {
@@ -37,11 +42,10 @@ private:
     } Vehicle;
 
     QVector<Gem> gems_;
-    Vehicle myVehicle_;
+    Vector2D myVehicle_;
     void addGem(int x, int y, int pts);
-    void printGem();
     void fillField(QString body);
-    QVector2D computeRelativePts(Gem g);
+    Vector2D computeRelativePts(Gem g);
     void computeMove();
 
 };

stomp.cpp

@@ -17,7 +17,7 @@ Stomp::Stomp() {
 
        case STOMPFrame::MESSAGE:
            notifyReceiveIndication(1, frame.headers().value("destination"), frame.body());
-           qDebug() << "Message" << Qt::endl;
+           //qDebug() << "Message" << Qt::endl;
            break;
 
        case STOMPFrame::RECEIPT: