JoystickControlWidget.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 *
 * @package    ArmarX::RobotAPI
 * @author     Raphael Grimm <raphael dot grimm at kit dot edu>
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "JoystickControlWidget.h"
 
//qt
#include <QBrush>
#include <QColor>
#include <QPen>
#include <QSizePolicy>
#include <QVBoxLayout>
 
//std
#include <cmath>
#include <iostream>
#include <memory>
 
namespace armarx
{
 
    JoystickControlWidgetQGraphicsView::JoystickControlWidgetQGraphicsView(QGraphicsScene* scene,
                                                                           QWidget* parent) :
        QGraphicsView{scene, parent}
    {
    }
 
    JoystickControlWidgetQGraphicsView::~JoystickControlWidgetQGraphicsView() = default;
 
    void
    JoystickControlWidgetQGraphicsView::mousePressEvent(QMouseEvent* event)
    {
        emit positionChanged(mapToScene(event->pos()));
        emit pressed();
    }
 
    void
    JoystickControlWidgetQGraphicsView::mouseMoveEvent(QMouseEvent* event)
    {
        emit positionChanged(mapToScene(event->pos()));
    }
 
    void
    JoystickControlWidgetQGraphicsView::mouseReleaseEvent(QMouseEvent*)
    {
        emit positionChanged({0, 0});
        emit released();
    }
 
    JoystickControlWidget::JoystickControlWidget(bool useQuadrant3and4, QWidget* parent) :
        QWidget{parent}, onlyQuadrant2and1{!useQuadrant3and4}, nibble{}, steps{0}
    {
        //build scene + use explicit pens (on some platforms the default ones dont have the desired default values)
        std::unique_ptr<QGraphicsScene> scene{new QGraphicsScene{}};
        QPen pen{Qt::black, 0};
        //bounding circle
        scene->addEllipse(-1, -1, 2, 2, pen);
        //inner circle
        scene->addEllipse(-0.13, -0.13, 0.26, 0.26, pen);
        //cross
        scene->addLine(0, -1, 0, 1, pen);
        scene->addLine(-1, 0, 1, 0, pen);
        //nibble
        QPen penRed{Qt::darkRed};
        penRed.setCosmetic(true);
        nibble = scene->addEllipse(-0.1, -0.1, 0.2, 0.2, penRed, QBrush{Qt::darkRed});
 
        //build view
        std::unique_ptr<JoystickControlWidgetQGraphicsView> viewPtr{
            new JoystickControlWidgetQGraphicsView{scene.release()}};
        view = viewPtr.get();
 
        view->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
        view->setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
        view->setSceneRect(-1, -1, 2, (onlyQuadrant2and1 ? 1 : 2));
 
        //connect
        connect(viewPtr.get(), SIGNAL(positionChanged(QPointF)), this, SLOT(setNibble(QPointF)));
        connect(viewPtr.get(), SIGNAL(released()), this, SLOT(mouseReleased()));
        connect(viewPtr.get(), SIGNAL(pressed()), this, SIGNAL(pressed()));
 
        //set gui
        std::unique_ptr<QVBoxLayout> layout{new QVBoxLayout{}};
        layout->setSpacing(0);
        layout->setContentsMargins(0, 0, 0, 0);
        layout->addWidget(viewPtr.release());
        setLayout(layout.release());
    }
 
    QPointF
    JoystickControlWidget::getPosition() const
    {
        return nibble->pos();
    }
 
    double
    JoystickControlWidget::getRotation() const
    {
        //if the nibble is in the middle return the angle 0.
        //otherwise use polar coordinates (up is 0, down pi, Quadrant I & IV are positive)
        auto pos = getPosition();
        auto length = std::hypot(pos.x(), pos.y());
        auto normalized = pos / length;
 
        if (length == 0)
        {
            return 0;
        }
        else
        {
            if (normalized.x() < 0)
            {
                return -std::acos(-normalized.y());
            }
            else
            {
                return std::acos(-normalized.y());
            }
        }
    }
 
    void
    JoystickControlWidget::mouseReleased()
    {
        setNibble({0, 0});
        emit released();
    }
 
    void
    JoystickControlWidget::setNibble(QPointF pos)
    {
        if (onlyQuadrant2and1 && (pos.y() > 0))
        {
            //project position to quadrant II & I
            pos.setY(0);
        }
 
        auto length = std::hypot(pos.x(), pos.y());
        auto normalized = pos / length;
 
        if (length != 0)
        {
 
            if (steps == 0)
            {
                //dont snap
                pos = (length > 1) ? normalized : pos;
            }
            else
            {
                //snapp to step
                double stepSize = 1.0 / steps;
                //length/(1.0/steps) => length*steps
                // +0.3*stepSize => if the border of the step is more forgiving
                double newLength =
                    static_cast<unsigned int>(length * steps + 0.3 * stepSize) * stepSize;
                pos = std::fmin(1, newLength) * normalized;
            }
        }
 
 
        //set position
        nibble->setPos(pos);
 
        //flip y (from downwards to upwards)
        pos.setY(-pos.y());
 
        //emit signals
        emit positionChanged(pos);
 
        emit rotationChanged(getRotation());
 
        emit changed(pos, getRotation());
    }
 
    void
    JoystickControlWidget::resizeEvent(QResizeEvent*)
    {
        int width;
        int height;
 
        //calculate size
        if (onlyQuadrant2and1)
        {
            height = std::min(contentsRect().width() / 2, contentsRect().height());
            width = height * 2;
        }
        else
        {
            width = std::min(contentsRect().width(), contentsRect().height());
            height = width;
        }
 
        //if minsz=maxsz you block resizing
        view->setMinimumSize(0.9 * width, 0.9 * height);
        view->setMaximumSize(width, height);
        auto viewScaleFactor = 0.49 * width;
        view->setTransform(QTransform::fromScale(viewScaleFactor, viewScaleFactor));
    }
 
    void
    JoystickControlWidget::setSteps(int stepCount)
    {
        steps = (stepCount < 0) ? 0 : stepCount;
    }
 
    int
    JoystickControlWidget::getSteps()
    {
        return steps;
    }
} // namespace armarx