diff --git a/CMakeLists.txt b/CMakeLists.txt index 74d79e3..74bd1a6 100644 --- a/CMakeLists.txt +++ b/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} ) diff --git a/UsersREMI~1.HERAppDataLocalTemptmp8h6pg12m b/UsersREMI~1.HERAppDataLocalTemptmp8h6pg12m deleted file mode 100644 index e69de29..0000000 diff --git a/Vector2D.cpp b/Vector2D.cpp new file mode 100644 index 0000000..b2889e8 --- /dev/null +++ b/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); +} \ No newline at end of file diff --git a/Vector2D.h b/Vector2D.h new file mode 100644 index 0000000..2997e03 --- /dev/null +++ b/Vector2D.h @@ -0,0 +1,41 @@ +#ifndef VECTOR2D_H +#define VECTOR2D_H + +#include + +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 diff --git a/app.cpp b/app.cpp index ec96429..b55728b 100644 --- a/app.cpp +++ b/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 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.xsendRequest("/topic/sdi10.gem.command", "left"); - else if(myGem.y>myVehicle_.y) st_->sendRequest("/topic/sdi10.gem.command", "down"); - else if(myGem.ysendRequest("/topic/sdi10.gem.command", "up"); } diff --git a/app.h b/app.h index 0b3d136..dab66aa 100644 --- a/app.h +++ b/app.h @@ -5,12 +5,19 @@ #include "interface/iStompObserver.h" #include "stomp.h" #include +#include + +#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 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(); }; diff --git a/stomp.cpp b/stomp.cpp index 8f4288b..38a4616 100644 --- a/stomp.cpp +++ b/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: