src/robomon/MclPainter.cpp

   1 /*
   2  * MclPainter.cpp                               08/01/26
   3  *
   4  * MCL painter.
   5  *
   6  * Copyright: (c) 2008 CTU Dragons
   7  *            CTU FEE - Department of Control Engineering
   8  * Authors: Martin Zidek, Michal Sojka, Tran Duy Khanh
   9  * License: GNU GPL v.2
  10  */
  11
  12 #include <QtGui>
  13 #include "MclPainter.h"
  14 #include <robomath.h>
  15
  16 MclPainter::MclPainter(struct mcl_model *mcl, int *displayCount)
  17 {
  18         background = QBrush(QColor(240, 220, 220));
  19         this->mcl = mcl;
  20         this->displayCount = displayCount;
  21 }
  22
  23 MclPainter::~MclPainter()
  24 {
  25
  26 }
  27
  28 void MclPainter::paint(QPainter *painter, QPaintEvent *event)
  29 {
  30         struct mcl_laser *l = mcl_to_laser(mcl);
  31         struct mcl_laser_state *parts = l->parts;
  32
  33         int count = mcl->count - *displayCount;
  34         int col;
  35         QRect rect = event->rect();
  36
  37         painter->fillRect(rect, background);
  38         painter->setPen(QColor(20, 20, 20));
  39
  40         for (int i=count; i<mcl->count; i++) {
  41                 /* draw a circle */
  42                 painter->translate(
  43                         (qreal)(parts[i].x / PLAYGROUND_WIDTH_M * width),
  44                         (qreal)((PLAYGROUND_HEIGHT_M-parts[i].y)
  45                                 / PLAYGROUND_HEIGHT_M * height));
  46
  47 //              col = (int)(parts[i].weight * 128 + 50);
  48 //              col = (col > 255) ? 255 : col;
  49 //              col = (col < 0) ? 0 : col;
  50                 col = 255;
  51
  52                 painter->setBrush(QColor(col, 40, 40));
  53                 painter->drawEllipse(0, 0, 10, 10);
  54                 painter->translate(
  55                         -(qreal)(parts[i].x / PLAYGROUND_WIDTH_M * width),
  56                         -(qreal)((PLAYGROUND_HEIGHT_M-parts[i].y)
  57                                 / PLAYGROUND_HEIGHT_M * height));
  58
  59                 /* draw a line representing the robot's head */
  60                 painter->translate(
  61                         (qreal)((parts[i].x / PLAYGROUND_WIDTH_M * width)+5),
  62                         (qreal)(((PLAYGROUND_HEIGHT_M-parts[i].y)
  63                                 / PLAYGROUND_HEIGHT_M * height)+5));
  64                 painter->rotate(RAD2DEG(-parts[i].angle));
  65                 painter->drawLine(0, 0, 20, 0);
  66                 painter->rotate(RAD2DEG(parts[i].angle));
  67                 painter->translate(
  68                         -(qreal)((parts[i].x / PLAYGROUND_WIDTH_M * width)+5),
  69                         -(qreal)(((PLAYGROUND_HEIGHT_M-parts[i].y)
  70                                 / PLAYGROUND_HEIGHT_M * height)+5));
  71         }
  72
  73         /* draw a circle */
  74         painter->translate(
  75                 (qreal)(estimated.x / PLAYGROUND_WIDTH_M * width),
  76                 (qreal)((PLAYGROUND_HEIGHT_M-estimated.y)
  77                         / PLAYGROUND_HEIGHT_M * height));
  78
  79         painter->setBrush(QColor(40, 40, 255));
  80         painter->drawEllipse(0, 0, 20, 20);
  81         painter->translate(
  82                 -(qreal)(estimated.x / PLAYGROUND_WIDTH_M * width),
  83                 -(qreal)((PLAYGROUND_HEIGHT_M-estimated.y)
  84                         / PLAYGROUND_HEIGHT_M * height));
  85
  86         /* draw a line representing the robot's head */
  87         painter->translate(
  88                 (qreal)((estimated.x / PLAYGROUND_WIDTH_M * width)+10),
  89                 (qreal)(((PLAYGROUND_HEIGHT_M-estimated.y)
  90                         / PLAYGROUND_HEIGHT_M * height)+10));
  91         painter->rotate(RAD2DEG(-estimated.angle));
  92         painter->drawLine(0, 0, 80, 0);
  93         painter->rotate(RAD2DEG(estimated.angle));
  94         painter->translate(
  95                 -(qreal)((estimated.x / PLAYGROUND_WIDTH_M * width)+10),
  96                 -(qreal)(((PLAYGROUND_HEIGHT_M-estimated.y)
  97                         / PLAYGROUND_HEIGHT_M * height)+10));
  98 }
  99
 100 void MclPainter::setSize(int width, int height)
 101 {
 102         this->width = width;
 103         this->height = height;
 104 }
 105
 106 QSize MclPainter::size()
 107 {
 108         return QSize(width, height);
 109 }