MAV2DIcon.cc 10.2 KB
Newer Older
1 2 3
#include "MAV2DIcon.h"
#include <QPainter>

4 5
#include <qmath.h>

6 7
#include "QGC.h"

8 9 10 11 12 13 14 15 16
MAV2DIcon::MAV2DIcon(mapcontrol::MapGraphicItem* map,mapcontrol::OPMapWidget* parent, UASInterface* uas, int radius, int type)
    : UAVItem(map,parent),
    yaw(0.0f),
    radius(radius),
    type(type),
    airframe(uas->getAirframe()),
    iconColor(uas->getColor()),
    selected(uas->getSelected()),
    uasid(uas->getUASID())
17
{
18
    size = QSize(radius, radius);
19 20
    pic = QPixmap(size);
    drawIcon();
21 22
}

23
MAV2DIcon::MAV2DIcon(mapcontrol::MapGraphicItem* map, mapcontrol::OPMapWidget* parent, qreal lat, qreal lon, qreal alt, QColor color)
24 25 26 27
    : UAVItem(map,parent),
    radius(20),
    type(0),
    airframe(0),
28
    iconColor(color),
29 30
    selected(false),
    uasid(0)
31 32
{
    size = QSize(radius, radius);
33 34 35
    pic = QPixmap(size);
    drawIcon();
    SetUAVPos(internals::PointLatLng(lat, lon), alt, color);
36 37 38 39
}

MAV2DIcon::~MAV2DIcon()
{
40
    //delete pic;
41 42
}

43 44 45
void MAV2DIcon::setSelectedUAS(bool selected)
{
    this->selected = selected;
46
    drawIcon();
47 48
}

49
/**
50 51
 * Yaw changes of less than ±15 degrees are ignored.
 *
52
 * @param yaw in radians, 0 = north, positive = clockwise
53
 */
54
void MAV2DIcon::setYaw(float yaw)
55
{
pixhawk's avatar
pixhawk committed
56
    //qDebug() << "MAV2Icon" << yaw;
57
    float diff = fabs(yaw - this->yaw);
58
    while (diff > (float)M_PI) {
59
        diff -= (float)M_PI;
60 61
    }

62
    if (diff > 0.1f) {
63
        this->yaw = yaw;
64
        drawIcon();
65
        // FIXME
66
    }
67 68
}

69
void MAV2DIcon::drawIcon()
70
{
71 72
    pic.fill(Qt::transparent);
    QPainter painter(&pic);
73 74 75 76
    painter.setRenderHint(QPainter::TextAntialiasing);
    painter.setRenderHint(QPainter::Antialiasing);
    painter.setRenderHint(QPainter::HighQualityAntialiasing);

77 78
    radius = qMin(pic.width(), pic.height());

79 80 81 82
    // Rotate by yaw
    painter.translate(radius/2, radius/2);

    // Draw selected indicator
83
    painter.setBrush(Qt::NoBrush);
84
    if (selected) {
85 86 87
        QPen pen(Qt::yellow);
        pen.setWidth(2);
        painter.setPen(pen);
88
    }
89 90 91 92 93 94 95
    else
    {
        QPen pen(Qt::white);
        pen.setWidth(1);
        painter.setPen(pen);
    }
    painter.drawEllipse(QPoint(0, 0), radius/2-1, radius/2-1);
96 97
    drawAirframePolygon(airframe, painter, radius, iconColor, yaw);
}
98

99 100
void MAV2DIcon::drawAirframePolygon(int airframe, QPainter& painter, int radius, QColor& iconColor, float yaw)
{
101
    switch (airframe) {
102
    case UASInterface::QGC_AIRFRAME_PREDATOR:
103
    case UASInterface::QGC_AIRFRAME_REAPER: {
104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151
            // DRAW AIRPLANE

            // Rotate 180 degs more since the icon is oriented downwards
            //float yawRotate = (yaw/(float)M_PI)*180.0f + 180.0f+180.0f;

            const float yawDeg = ((yaw/M_PI)*180.0f)+180.0f+180.0f;
            int yawRotate = static_cast<int>(yawDeg) % 360;

            painter.rotate(yawRotate);

            //qDebug() << "ICON SIZE:" << radius;

            float iconSize = radius*0.9f;
            QPolygonF poly(24);
            poly.replace(0, QPointF(0.000000f*iconSize, -0.312500f*iconSize));
            poly.replace(1, QPointF(0.025000f*iconSize, -0.287500f*iconSize));
            poly.replace(2, QPointF(0.037500f*iconSize, -0.237500f*iconSize));
            poly.replace(3, QPointF(0.031250f*iconSize, -0.187500f*iconSize));
            poly.replace(4, QPointF(0.025000f*iconSize, -0.043750f*iconSize));
            poly.replace(5, QPointF(0.500000f*iconSize, -0.025000f*iconSize));
            poly.replace(6, QPointF(0.500000f*iconSize, 0.025000f*iconSize));
            poly.replace(7, QPointF(0.025000f*iconSize, 0.043750f*iconSize));
            poly.replace(8, QPointF(0.025000f*iconSize, 0.162500f*iconSize));
            poly.replace(9, QPointF(0.137500f*iconSize, 0.181250f*iconSize));
            poly.replace(10, QPointF(0.137500f*iconSize, 0.218750f*iconSize));
            poly.replace(11, QPointF(0.025000f*iconSize, 0.206250f*iconSize));
            poly.replace(12, QPointF(-0.025000f*iconSize, 0.206250f*iconSize));
            poly.replace(13, QPointF(-0.137500f*iconSize, 0.218750f*iconSize));
            poly.replace(14, QPointF(-0.137500f*iconSize, 0.181250f*iconSize));
            poly.replace(15, QPointF(-0.025000f*iconSize, 0.162500f*iconSize));
            poly.replace(16, QPointF(-0.025000f*iconSize, 0.043750f*iconSize));
            poly.replace(17, QPointF(-0.500000f*iconSize, 0.025000f*iconSize));
            poly.replace(18, QPointF(-0.500000f*iconSize, -0.025000f*iconSize));
            poly.replace(19, QPointF(-0.025000f*iconSize, -0.043750f*iconSize));
            poly.replace(20, QPointF(-0.031250f*iconSize, -0.187500f*iconSize));
            poly.replace(21, QPointF(-0.037500f*iconSize, -0.237500f*iconSize));
            poly.replace(22, QPointF(-0.031250f*iconSize, -0.262500f*iconSize));
            poly.replace(23, QPointF(0.000000f*iconSize, -0.312500f*iconSize));

            painter.setBrush(QBrush(iconColor));
            QPen iconPen(Qt::black);
            iconPen.setWidthF(1.0f);
            painter.setPen(iconPen);

            painter.drawPolygon(poly);
            painter.rotate(-yawRotate);
        }
        break;
152
    case UASInterface::QGC_AIRFRAME_MIKROKOPTER:
153
    case UASInterface::QGC_AIRFRAME_CHEETAH: {
154 155 156 157
            // QUADROTOR
            float iconSize = radius*0.9f;
            const float yawDeg = ((yaw/M_PI)*180.0f)+180.0f;
            int yawRotate = static_cast<int>(yawDeg) % 360;
158

159
            painter.rotate(yawRotate);
160

161
            //qDebug() << "ICON SIZE:" << radius;
162

163 164 165 166 167 168 169 170
            QPointF front(0, 0.2);
            front = front *iconSize;
            QPointF left(-0.2, 0);
            left = left * iconSize;
            QPointF right(0.2, 0.0);
            right *= iconSize;
            QPointF back(0, -0.2);
            back *= iconSize;
171

172
            QPolygonF poly(0);
173 174 175



176 177 178 179
            painter.setBrush(QBrush(iconColor));
            QPen iconPen(Qt::black);
            iconPen.setWidthF(1.0f);
            painter.setPen(iconPen);
180

181
            painter.drawPolygon(poly);
182

183 184 185
            painter.drawEllipse(left, radius/4/2, radius/4/2);
            painter.drawEllipse(right, radius/4/2, radius/4/2);
            painter.drawEllipse(back, radius/4/2, radius/4/2);
186

187 188 189 190 191
            painter.setBrush(Qt::red);
            painter.drawEllipse(front, radius/4/2, radius/4/2);
            painter.rotate(-yawRotate);
        }
        break;
192 193 194
    case UASInterface::QGC_AIRFRAME_EASYSTAR:
    case UASInterface::QGC_AIRFRAME_MERLIN:
    case UASInterface::QGC_AIRFRAME_TWINSTAR:
195
    case UASInterface::QGC_AIRFRAME_PTERYX: {
196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248
            // DRAW AIRPLANE

            const float yawDeg = ((yaw/M_PI)*180.0f)+180.0f;
            int yawRotate = static_cast<int>(yawDeg) % 360;
            painter.rotate(yawRotate);

            //qDebug() << "ICON SIZE:" << radius;

            float iconSize = radius*0.7f;

            QPolygonF poly(32);
            poly.replace(0, QPointF(0.000000f*iconSize, 0.362319f*iconSize));
            poly.replace(1, QPointF(0.018116f*iconSize, 0.340580f*iconSize));
            poly.replace(2, QPointF(0.028986f*iconSize, 0.318841f*iconSize));
            poly.replace(3, QPointF(0.036232f*iconSize, 0.166667f*iconSize));
            poly.replace(4, QPointF(0.326087f*iconSize, 0.170290f*iconSize));
            poly.replace(5, QPointF(0.420290f*iconSize, 0.130435f*iconSize));
            poly.replace(6, QPointF(0.456522f*iconSize, 0.108696f*iconSize));
            poly.replace(7, QPointF(0.500000f*iconSize, 0.000000f*iconSize));
            poly.replace(8, QPointF(0.456522f*iconSize, 0.021739f*iconSize));
            poly.replace(9, QPointF(0.391304f*iconSize, 0.021739f*iconSize));
            poly.replace(10, QPointF(0.028986f*iconSize, 0.021739f*iconSize));
            poly.replace(11, QPointF(0.021739f*iconSize, -0.239130f*iconSize));
            poly.replace(12, QPointF(0.094203f*iconSize, -0.246377f*iconSize));
            poly.replace(13, QPointF(0.123188f*iconSize, -0.268116f*iconSize));
            poly.replace(14, QPointF(0.144928f*iconSize, -0.304348f*iconSize));
            poly.replace(15, QPointF(0.000000f*iconSize, -0.307971f*iconSize));
            poly.replace(16, QPointF(0.000000f*iconSize, -0.307971f*iconSize));
            poly.replace(17, QPointF(-0.144928f*iconSize, -0.304348f*iconSize));
            poly.replace(18, QPointF(-0.123188f*iconSize, -0.268116f*iconSize));
            poly.replace(19, QPointF(-0.094203f*iconSize, -0.246377f*iconSize));
            poly.replace(20, QPointF(-0.021739f*iconSize, -0.239130f*iconSize));
            poly.replace(21, QPointF(-0.028986f*iconSize, 0.021739f*iconSize));
            poly.replace(22, QPointF(-0.391304f*iconSize, 0.021739f*iconSize));
            poly.replace(23, QPointF(-0.456522f*iconSize, 0.021739f*iconSize));
            poly.replace(24, QPointF(-0.500000f*iconSize, 0.000000f*iconSize));
            poly.replace(25, QPointF(-0.456522f*iconSize, 0.108696f*iconSize));
            poly.replace(26, QPointF(-0.420290f*iconSize, 0.130435f*iconSize));
            poly.replace(27, QPointF(-0.326087f*iconSize, 0.170290f*iconSize));
            poly.replace(28, QPointF(-0.036232f*iconSize, 0.166667f*iconSize));
            poly.replace(29, QPointF(-0.028986f*iconSize, 0.318841f*iconSize));
            poly.replace(30, QPointF(-0.018116f*iconSize, 0.340580f*iconSize));
            poly.replace(31, QPointF(-0.000000f*iconSize, 0.362319f*iconSize));

            painter.setBrush(QBrush(iconColor));
            QPen iconPen(Qt::black);
            iconPen.setWidthF(1.0f);
            painter.setPen(iconPen);

            painter.drawPolygon(poly);
            painter.rotate(-yawRotate);
        }
        break;
249 250
    case UASInterface::QGC_AIRFRAME_GENERIC:
    default: {
251
            // GENERIC
252

253
            float yawRotate = (yaw/(float)M_PI)*180.0f + 180.0f;
254

255
            painter.rotate(yawRotate);
256

257
            //qDebug() << "ICON SIZE:" << radius;
258

259 260 261 262 263
            float iconSize = radius*0.9f;
            QPolygonF poly(3);
            poly.replace(0, QPointF(0.0f*iconSize, 0.3f*iconSize));
            poly.replace(1, QPointF(-0.2f*iconSize, -0.2f*iconSize));
            poly.replace(2, QPointF(0.2f*iconSize, -0.2f*iconSize));
264

265 266 267 268
            painter.setBrush(QBrush(iconColor));
            QPen iconPen(Qt::black);
            iconPen.setWidthF(1.0f);
            painter.setPen(iconPen);
269

270 271 272 273
            painter.drawPolygon(poly);
            painter.rotate(-yawRotate);
        }
        break;
274
    }
275
}