129 lines
3.2 KiB
C++
129 lines
3.2 KiB
C++
#include "app.h"
|
|
|
|
|
|
App::App(Stomp* st) {
|
|
st_ = st;
|
|
st_->connectRequest("sdi.hevs.ch", 61614, "/", "sdi10", "809c02f36becb0868da98761fe3209f6");
|
|
st_->subscribeRequest("/topic/sdi10.gem.field", 1);
|
|
|
|
}
|
|
|
|
void App::connectConfirmation(bool success, QString version) {
|
|
|
|
}
|
|
|
|
void App::sendConfirmation(bool success) {
|
|
|
|
}
|
|
|
|
void App::subscribeConfirmation(bool success) {
|
|
st_->sendRequest("/topic/sdi10.gem.command", "up");
|
|
}
|
|
|
|
void App::receiveIndication(int id, QString destination, QString body) {
|
|
//qDebug() << "Indication " << id << " : " << destination << Qt::endl << body << Qt::endl;
|
|
if(destination.contains("field")){
|
|
fillField(body);
|
|
}
|
|
computeMove();
|
|
}
|
|
|
|
void App::disconnectConfirmation() {
|
|
|
|
}
|
|
|
|
void App::disconnectIndication() {
|
|
|
|
}
|
|
|
|
void App::addGem(int x, int y, int pts) {
|
|
Gem g;
|
|
g.coordinate = Vector2D(Vector2D::CreatePoint(x, GRID_SIZE-y));
|
|
g.pts = pts;
|
|
gems_.append(g);
|
|
}
|
|
|
|
void App::fillField(QString body) {
|
|
int x = 0;
|
|
int y = 0;
|
|
gems_.clear();
|
|
for(int i = 0; i<body.length(); i++) {
|
|
const QChar c = body.at(i);
|
|
if(c == 'Y') {
|
|
myVehicle_ = Vector2D(Vector2D::CreatePoint(x, GRID_SIZE-y));
|
|
}
|
|
if(c == 'g') addGem(x, y, 100);
|
|
if(c == 'G') addGem(x, y, 250);
|
|
if(c == 'D') addGem(x, y, 500);
|
|
if(c == '\n') {
|
|
y++;
|
|
x = 0;
|
|
} else {
|
|
x++;
|
|
}
|
|
}
|
|
}
|
|
|
|
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() {
|
|
static const double PI = 3.14159265358979323846;
|
|
Vector2D myRelativePts;
|
|
for(Gem g : gems_) {
|
|
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");
|
|
}
|
|
}
|
|
|