robot.cpp

#include "robot.h"
#include <math.h>
#include <QPen>
#include "playgroundscene.h"

robot::robot(QGraphicsItem *parent, QGraphicsScene *scene) : QGraphicsItem(parent)
{
        actPhi = M_PI/2;
        
        this->scene = scene;

        this->robotBody = new QGraphicsRectItem(QRectF(OFFSET_X,OFFSET_Y,ROBOT_SIZE_X,ROBOT_SIZE_Y),parent,scene);
        this->robotBody->setPen(QPen(QBrush(Qt::black),1,Qt::SolidLine,Qt::FlatCap,Qt::BevelJoin));
        this->robotBody->setBrush(QBrush(Qt::black));
        this->robotBody->setParentItem(this);
        this->robotBody->setZValue(10);         
                        ;
        this->sensShort[0] = new QGraphicsLineItem(OFFSET_X+2*2,OFFSET_Y,OFFSET_X+2*2,OFFSET_Y-30*2,parent,scene);
        
        this->sensShort[1] = new QGraphicsLineItem(OFFSET_X+9.5*2,OFFSET_Y,OFFSET_X+9.5*2,OFFSET_Y-30*2,parent,scene);
        
        this->sensShort[2] = new QGraphicsLineItem(OFFSET_X+17*2,OFFSET_Y,OFFSET_X+17*2,OFFSET_Y-30*2,parent,scene);
        
        this->sensShort[3] = new QGraphicsLineItem(OFFSET_X+28*2,OFFSET_Y,OFFSET_X+28*2,OFFSET_Y-30*2,parent,scene);
        
        this->sensShort[4] = new QGraphicsLineItem(OFFSET_X+25*2,OFFSET_Y+ROBOT_SIZE_Y,OFFSET_X+25*2,OFFSET_Y+ROBOT_SIZE_Y+30*2,parent,scene);
        
        this->sensShort[5] = new QGraphicsLineItem(OFFSET_X+5*2,OFFSET_Y+ROBOT_SIZE_Y,OFFSET_X+5*2,OFFSET_Y+ROBOT_SIZE_Y+30*2,parent,scene);
        
        for(int i=0;i<6;i++) {
                this->sensShort[i]->setParentItem(this);
                this->sensShort[i]->setPen(QPen(QBrush(Qt::red),2,Qt::DashLine,Qt::FlatCap,Qt::BevelJoin));
        }       

        this->sensLong[0] = new QGraphicsLineItem(OFFSET_X+5*2,OFFSET_Y,OFFSET_X+5*2,OFFSET_Y-80*2,parent,scene);

        this->sensLong[1] = new QGraphicsLineItem(OFFSET_X+15*2,OFFSET_Y,OFFSET_X+15*2,OFFSET_Y-80*2,parent,scene);

        this->sensLong[2] = new QGraphicsLineItem(OFFSET_X+25*2,OFFSET_Y,OFFSET_X+25*2,OFFSET_Y-80*2,parent,scene);
        
        this->setZValue(10);

//      for(int i=0; i<3; i++) {
//              beacons[i] = new QGraphicsLineItem(parent, scene);
// //           beacon1->setLine(QLineF(OFFSET_X+ROBOT_SIZE_X/2,OFFSET_Y+ROBOT_SIZE_Y/2,BEACON1_X,BEACON1_Y));
//              beacons[i]->setPen(QPen(QBrush(Qt::cyan),2,Qt::SolidLine,Qt::SquareCap,Qt::BevelJoin));
//              beacons[i]->setZValue(20);
//      }
//      beacon2 = new QGraphicsLineItem(parent, scene);
//      beacon2->setLine(QLineF(OFFSET_X+ROBOT_SIZE_X/2,OFFSET_Y+ROBOT_SIZE_Y/2,BEACON2_X,BEACON2_Y));
//      beacon2->setPen(QPen(QBrush(Qt::cyan),2,Qt::SolidLine,Qt::SquareCap,Qt::BevelJoin));
//      beacon2->setZValue(20);

//      beacon3 = new QGraphicsLineItem(parent, scene);
//      beacon3->setLine(QLineF(OFFSET_X+ROBOT_SIZE_X/2,OFFSET_Y+ROBOT_SIZE_Y/2,BEACON3_X,BEACON3_Y));
//      beacon3->setPen(QPen(QBrush(Qt::cyan),2,Qt::SolidLine,Qt::SquareCap,Qt::BevelJoin));
//      beacon3->setZValue(20);
        
        this->pCenter = new QPoint(OFFSET_X+ROBOT_SIZE_X/2, 420 - OFFSET_Y -ROBOT_SIZE_Y);
}

void robot::initBeacons() {
        
        setBeaconAngle(0,M_PI/4);
        beacons[0]->setLine(this->pos().x()+ROBOT_SIZE_X/2,this->pos().y()+ROBOT_SIZE_Y/2,BEACON1_X,BEACON1_Y);
        beacons[1]->setLine(this->pos().x()+ROBOT_SIZE_X/2,this->pos().y()+ROBOT_SIZE_Y/2,BEACON2_X,BEACON2_Y);
        beacons[2]->setLine(this->pos().x()+ROBOT_SIZE_X/2,this->pos().y()+ROBOT_SIZE_Y/2,BEACON3_X,BEACON3_Y);
}

void robot::setBeaconAngle(int idx, double angle) {
        double y,x;
        double c = 800;
        angle+=actPhi;
        y = c*(sin(angle));
        x = c*(cos(angle));
        if(angle>M_PI) y = -y;
        if((angle>M_PI/2)&&(angle<3*M_PI/2)) x = -x;
        printf("x %d, y %d\n",pCenter->x(),pCenter->y());
//      printf("x %f, y %f, c %f, sin %f, cos %f\n",a,b,c,sin(angle),cos(angle));
        beacons[idx]->setLine(0,0,(int)x,(int)y);
        beacons[idx]->moveBy(pCenter->x(),420-pCenter->y());
}

robot::~robot()
{
}

QRectF robot::boundingRect() const
 {
     return QRectF(this->pos().x(), this->pos().y(), ROBOT_SIZE_X+80, ROBOT_SIZE_Y+80);
 }

void robot::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget)
 {     
        Q_UNUSED(option);
        Q_UNUSED(widget);
//      Q_UNUSED(painter);
        
//      painter->setPen(QPen(QBrush(Qt::black),1,Qt::SolidLine,Qt::FlatCap,Qt::BevelJoin));
//      painter->setBrush(QBrush(Qt::black));
//      painter->drawRect(QRectF(OFFSET_X,OFFSET_Y,ROBOT_SIZE_X,ROBOT_SIZE_Y));
}

void robot::rotateRobot ( qreal angle )
{
        this->translate(ROBOT_SIZE_X/2,ROBOT_SIZE_Y/2);
        this->rotate(angle);
        this->translate(-ROBOT_SIZE_X/2,-ROBOT_SIZE_Y/2);
}

void robot::sensorRange(qreal size, int sens_id)
{       
        qreal x1 = this->sensShort[sens_id]->line().x1();
        qreal y1 = this->sensShort[sens_id]->line().y1();
        qreal x2 = this->sensShort[sens_id]->line().x2();
        qreal y2 = this->sensShort[sens_id]->line().y2();
        
        qreal dx = x1+size*(x2-x1);
        qreal dy = y1+size*(y2-y1);     

        this->sensShort[sens_id]->setLine(x1,y1,dx,dy);
}

void robot::moveRobot(double x, double y, double phi)
{
    QPointF pos(x,y);
    pos = PlaygroundScene::world2scene(pos);
    //TODO use this transpormation function
    
        this->setPos(x*100*2-(ROBOT_SIZE_X/2),420-ROBOT_SIZE_Y-(y*100*2)+(ROBOT_SIZE_Y/2));
//      this->setPos(x*100*2-(ROBOT_SIZE_X),420-ROBOT_SIZE_Y-(y*100*2)+(ROBOT_SIZE_Y/2));
        this->rotateRobot((actPhi-phi)*180/M_PI);
        actPhi = phi;
}

---