renderarea.cpp

Go to the documentation of this file.

/* Author: Constantinos Chamzas, Zachary Kingston */

#include <iostream>

#include <se2ez/core/log.h>
#include <se2ez/core/math.h>
#include <se2ez/core/robot.h>
#include <se2ez/core/state.h>
#include <se2ez/core/frame.h>
#include <se2ez/core/geometry.h>

#include <se2ez/gui/renderarea.h>
#include <se2ez/gui/mainwindow.h>

#include "ui_mainwindow.h"
#include "ui_renderarea.h"

using namespace se2ez;

///
/// Geometry Drawers
///

gui::RenderArea::GeometryDrawer::GeometryDrawer(const RobotPtr &robot, const StatePtr &state)
  : robot_(robot), state_(state)
{
}

gui::RenderArea::CollideDrawer::CollideDrawer(bool doCollide, const std::set<std::string> &colliding,
                                              const RobotPtr &robot, const StatePtr &state)
  : gui::RenderArea::GeometryDrawer(robot, state), doCollide(doCollide), colliding(colliding)
{
}

gui::RenderArea::CollideDrawer::~CollideDrawer()
{
}

bool gui::RenderArea::CollideDrawer::frameCallback(RenderArea * /*area*/, QPainter & /*painter*/,
                                                   const FramePtr & /*frame*/)
{
    return true;
}

bool gui::RenderArea::CollideDrawer::geometryCallback(RenderArea * /*area*/, QPainter &painter,
                                                      const FramePtr &frame, const GeometryPtr & /*geometry*/)
{
    if (doCollide && colliding.find(frame->getName()) != colliding.end())
    {
        QPen pen(QColor(255, 0, 0));
        pen.setWidth(3);
        painter.setPen(pen);
    }

    return true;
}

gui::RenderArea::SignedDistanceDrawer::SignedDistanceDrawer(
    bool doCollide, bool doMin, const CollisionManager::SignedDistance &minimum,
    const CollisionManager::SignedDistanceMap &distances, const RobotPtr &robot, const StatePtr &state)
  : gui::RenderArea::GeometryDrawer(robot, state)
  , doCollide(doCollide)
  , doMin(doMin)
  , minimum(minimum)
  , distances(distances)
{
}

gui::RenderArea::SignedDistanceDrawer::~SignedDistanceDrawer()
{
}

bool gui::RenderArea::SignedDistanceDrawer::frameCallback(RenderArea * /*area*/, QPainter & /*painter*/,
                                                          const FramePtr & /*frame*/)
{
    return true;
}

bool gui::RenderArea::SignedDistanceDrawer::geometryCallback(RenderArea *area, QPainter &painter,
                                                             const FramePtr &frame,
                                                             const GeometryPtr & /*geometry*/)
{
    auto f = distances.find(frame->getName());
    if (f != distances.end())
    {
        const CollisionManager::SignedDistance &sd = f->second;

        if ((doMin && frame->getName() == minimum.frame) || not doMin)
        {
            painter.save();
            painter.resetTransform();

            QPen pen(QColor(255, 0, 0));
            pen.setWidth(2);
            painter.setPen(pen);

            int x, y, x2, y2, x3, y3, xt, yt;
            area->toScreen(sd.point[0], sd.point[1],  //
                           x, y);
            area->toScreen(sd.point[0] + sd.normal[0],  //
                           sd.point[1] + sd.normal[1],  //
                           x2, y2);
            area->toScreen(sd.point[0] + sd.normal[0] * 0.9,  //
                           sd.point[1] + sd.normal[1] * 0.9,  //
                           x3, y3);
            area->toScreen(sd.point[0] + sd.normal[0] * 0.5,  //
                           sd.point[1] + sd.normal[1] * 0.5,  //
                           xt, yt);

            painter.drawLine(x, y, x2, y2);
            painter.drawText(xt + 5, yt, QString(log::format("%1$+3.2f", sd.distance).c_str()));

            pen.setWidth(4);
            painter.setPen(pen);
            painter.drawLine(x2, y2, x3, y3);

            painter.restore();
        }

        // Paint collisions red still
        if (doCollide && sd.distance < 0)
        {
            QPen pen(QColor(255, 0, 0));
            pen.setWidth(3);
            painter.setPen(pen);
        }
    }

    return true;
}

///
/// Render Area
///

gui::RenderArea::RenderArea(QWidget *parent, MainWindow *mainwindow)
  : QWidget(parent), ui_(new Ui::RenderArea), mainwidget_(mainwindow)
{
    // ui_->setupUi(this);
    setMouseTracking(true);

    QFont newFont = font();
    newFont.setPixelSize(12);
    setFont(newFont);

    QFontMetrics fontMetrics(newFont);
    xBoundingRect_ = fontMetrics.boundingRect(tr("x"));
    yBoundingRect_ = fontMetrics.boundingRect(tr("y"));

    setOriginX(x_);
    setOriginY(y_);
}

gui::RenderArea::~RenderArea()
{
    delete ui_;
}

void gui::RenderArea::setOriginX(double x)
{
    x_ = x;
    origx_ = RATIO / scale_ * x_;

    mainwidget_->ui_->xSpinBox->blockSignals(true);
    mainwidget_->ui_->xSpinBox->setValue(x_);
    mainwidget_->ui_->xSpinBox->blockSignals(false);
}

void gui::RenderArea::setOriginY(double y)
{
    y_ = y;
    origy_ = RATIO / scale_ * y_;

    mainwidget_->ui_->ySpinBox->blockSignals(true);
    mainwidget_->ui_->ySpinBox->setValue(y_);
    mainwidget_->ui_->ySpinBox->blockSignals(false);
}

void gui::RenderArea::setScale(double scale)
{
    if (scale < SCALE_BOUNDS[0])
        scale = SCALE_BOUNDS[0];
    if (scale > SCALE_BOUNDS[1])
        scale = SCALE_BOUNDS[1];

    scale_ = scale;
    setOriginX(x_);
    setOriginY(y_);

    mainwidget_->ui_->zoomSpinBox->blockSignals(true);
    mainwidget_->ui_->zoomSpinBox->setValue(scale);
    mainwidget_->ui_->zoomSpinBox->blockSignals(false);
}

void gui::RenderArea::drawAllFrames(QPainter &painter, RobotPtr robot, StatePtr state)
{
    for (const auto &name : robot->getFrameNames())
    {
        const auto &fpose = tf::flattenIsometry(state->getPose(robot, name));

        painter.save();
        QTransform t;
        t.translate(fpose[0] * scale_ * ROBOT, fpose[1] * scale_ * ROBOT);
        t.rotateRadians(fpose[2]);
        painter.setTransform(t, true);
        drawFrame(painter, QString::fromUtf8(name.c_str()));
        painter.restore();
    }
}

void gui::RenderArea::drawGeometry(QPainter &painter, RobotPtr robot, StatePtr state,
                                   gui::RenderArea::GeometryDrawerPtr drawer)
{
    for (const auto &name : robot->getFrameNames())
    {
        const auto &fpose = tf::flattenIsometry(state->getPose(robot, name));
        const auto &frame = robot->getFrameConst(name);

        // Performing the frame transform
        painter.save();
        QTransform t;
        t.translate(fpose[0] * ROBOT * scale_, fpose[1] * ROBOT * scale_);
        t.rotateRadians(fpose[2]);
        painter.setTransform(t, true);

        if (drawer && !drawer->frameCallback(this, painter, frame))
            continue;

        for (const auto &geom : frame->getGeometry())
        {
            const auto &color = geom->color;
            const auto &offset = tf::flattenIsometry(geom->offset);

            // Performing the offset transform
            painter.save();
            QTransform offt;
            offt.translate(offset[0] * ROBOT * scale_, offset[1] * ROBOT * scale_);
            offt.rotateRadians(offset[2]);
            painter.setTransform(offt, true);

            // Converting floats to RGBD colors
            painter.setPen(QColor(255 * color[0], 255 * color[1], 255 * color[2], 255 * color[3]));

            if (drawer && !drawer->geometryCallback(this, painter, frame, geom))
                continue;

            switch (geom->type)
            {
                case Geometry::BOX:
                    painter.drawRect(
                        -scale_ * ROBOT * (geom->dimensions[0]), -scale_ * ROBOT * (geom->dimensions[1]),
                        scale_ * 2 * ROBOT * geom->dimensions[0], scale_ * 2 * ROBOT * geom->dimensions[1]);
                    break;

                case Geometry::CIRCLE:
                    painter.drawEllipse(QPoint(0, 0), int(scale_ * ROBOT * geom->dimensions[0]),
                                        int(scale_ * ROBOT * geom->dimensions[0]));
                    break;

                case Geometry::CONVEX:
                case Geometry::SIMPLE:
                {
                    QPoint points[geom->points.size()];
                    for (unsigned int i = 0; i < geom->points.size(); ++i)
                        points[i] =
                            QPoint(geom->points[i][0] * scale_ * ROBOT, geom->points[i][1] * scale_ * ROBOT);

                    painter.drawPolygon(points, geom->points.size());
                    break;
                }
                default:
                    SE2EZ_ERROR("Geometry Type: %d does not exist", geom->type);
            }

            painter.restore();
        }

        painter.restore();
    }
}

void gui::RenderArea::bottomLeftTransform(QPainter &painter)
{
    // TODO: make this compatible with zoom
    QPoint p = painter.window().bottomLeft();
    QTransform t;
    t.translate(p.x() + origx_ * scale_, p.y() - origy_ * scale_);
    t.scale(1, -1);
    painter.setTransform(t);
}

void gui::RenderArea::drawGrid(QPainter &painter)
{
    if (!mainwidget_->ui_->grid->isChecked())
        return;

    QRect vp = painter.viewport();

    int vx, vy, tx, ty;
    double vxd, vyd, txd, tyd;
    vp.getCoords(&vx, &vy, &tx, &ty);

    toRobot(vx, vy, vxd, vyd);
    toRobot(tx, ty, txd, tyd);

    vxd = floor(vxd);
    vyd = floor(vyd);
    txd = ceil(txd);
    tyd = floor(tyd);

    // SE2EZ_INFORM("%1% %2%", vxd, vyd);
    // SE2EZ_INFORM("%1% %2%", txd, tyd);

    const double move = 1;
    for (double s = vxd; s <= txd; s += move)
    {
        if (fabs(s) <= math::eps)
            painter.setPen(QColor(220, 220, 220));
        else
            painter.setPen(QColor(240, 240, 240));

        int x, y;
        toScreen(s, 0, x, y);

        painter.drawLine(x, 0, x, vp.height());

        painter.setPen(QColor(180, 180, 180));
        painter.drawText(x + 2, vp.height() - 2, QString(log::format("%1$+3.0f", s).c_str()));
    }

    for (double s = tyd; s <= vyd; s += move)
    {
        if (fabs(s) <= math::eps)
            painter.setPen(QColor(220, 220, 220));
        else
            painter.setPen(QColor(240, 240, 240));

        int x, y;
        toScreen(0, s, x, y);

        painter.drawLine(0, y, vp.width(), y);

        painter.setPen(QColor(180, 180, 180));
        painter.drawText(2, y - 2, QString(log::format("%1$+3.0f", s).c_str()));
    }
}

void gui::RenderArea::paint(bool force)
{
    paint_ = true;
    if (force)
    {
        // SE2EZ_WARN("Forcing repaint");
        // mainwidget_->resize(mainwidget_->width() + 1, mainwidget_->height());
        // resize(width(), height());

        // mainwidget_->update();
        // update();
        // QCoreApplication::processEvents();
        // mainwidget_->resize(mainwidget_->width() - 1, mainwidget_->height());
    }
    else
        update();
}

bool gui::RenderArea::didPaint()
{
    return !paint_;
}

void gui::RenderArea::paintEvent(QPaintEvent *event)
{
    QRect size = geometry();
    pixmap_ = QPixmap(size.size());
    QPainter painter(&pixmap_);

    // QPainter painter(this);
    painter.setRenderHint(QPainter::Antialiasing);
    painter.fillRect(event->rect(), QBrush(Qt::white));

    drawGrid(painter);

    bottomLeftTransform(painter);

    // Delegate all the drawing to the mainwidget
    mainwidget_->draw(painter, this);

    QPainter painter2(this);
    painter2.drawPixmap(size, pixmap_, size);
    paint_ = false;
}

void gui::RenderArea::drawFrame(QPainter &painter, QString name)
{
    QPen pen(QColor(200, 20, 20));
    pen.setWidth(2);
    painter.setPen(pen);

    painter.drawLine(0, 0, ROBOT, 0);
    painter.drawLine(ROBOT - 2, -2, ROBOT, 0);
    painter.drawLine(ROBOT - 2, 2, ROBOT, 0);

    painter.drawLine(0, 0, 0, ROBOT);
    painter.drawLine(-2, ROBOT - 2, 0, ROBOT);
    painter.drawLine(2, ROBOT - 2, 0, ROBOT);

    QTransform t = painter.transform();

    painter.save();
    painter.resetTransform();

    QPoint x(ROBOT - xBoundingRect_.width(),  //
             -5 - xBoundingRect_.height() / 2);
    QPoint y(-5 - yBoundingRect_.width(),  //
             ROBOT - yBoundingRect_.height() / 2);

    painter.drawText(t.map(x), "x");
    painter.drawText(t.map(y), "y");

    painter.drawText(t.map(QPoint(10, 10)), name);

    painter.restore();
}

void gui::RenderArea::toRobot(int xin, int yin, double &xout, double &yout)
{
    xout = ((xin) / scale_ - origx_) / ROBOT;
    yout = ((height() - yin) / scale_ - origy_) / ROBOT;
}

void gui::RenderArea::toScreen(double xin, double yin, int &xout, int &yout)
{
    xout = scale_ * (xin * ROBOT + origx_);
    yout = -(scale_ * (yin * ROBOT + origy_) - height());
}

void gui::RenderArea::mousePressEvent(QMouseEvent *event)
{
    QPoint pos = event->pos();

    if (event->button() == Qt::LeftButton)
    {
        click_ = pos;
        panning_ = true;
    }
    else
    {
        double x, y;
        toRobot(pos.x(), pos.y(), x, y);
        mainwidget_->click(event, x, y, pos.x(), pos.y());
    }

    event->accept();
}

void gui::RenderArea::mouseMoveEvent(QMouseEvent *event)
{
    QPoint pos = event->pos();

    double x, y;
    toRobot(pos.x(), pos.y(), x, y);
    mainwidget_->ui_->coords->setText(QString(log::format("%1$+6.2f, %2$+6.2f", x, y).c_str()));

    if ((event->buttons() & Qt::LeftButton) && panning_)
    {
        setOriginX(x_ + (1 / RATIO) * (pos.x() - click_.x()));
        setOriginY(y_ - (1 / RATIO) * (pos.y() - click_.y()));

        click_ = pos;
        mainwidget_->updateState();
    }

    if ((event->buttons() & Qt::RightButton))
    {
        double x, y;
        toRobot(pos.x(), pos.y(), x, y);
        mainwidget_->click(event, x, y, pos.x(), pos.y());
    }

    event->accept();
}

void gui::RenderArea::mouseReleaseEvent(QMouseEvent *event)
{
    if (event->button() == Qt::LeftButton && panning_)
        panning_ = false;

    event->accept();
}

void gui::RenderArea::wheelEvent(QWheelEvent *event)
{
    double delta = event->angleDelta().y() / 1000.;
    setScale(scale_ + delta);

    event->accept();
    mainwidget_->updateState();
}

void gui::RenderArea::takeScreenshot(const std::string &file)
{
    pixmap_.save(QString(file.c_str()));
}