Transition.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
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include <math.h>
 
#include <string>
#include <utility>
 
#include <boost/lexical_cast.hpp>
 
#include <QGraphicsPolygonItem>
#include <QPolygonF>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include "StateChartGraphEdge.h"
 
#define pi 3.14159265
 
//TODO: implement as copy constructor
StateChartGraphEdge::StateChartGraphEdge(const StateChartGraphEdge&) :
    QGraphicsPathItem(0, 0), m_startControlPoint(), m_endControlPoint()
{
}
 
StateChartGraphEdge::StateChartGraphEdge(QGraphicsScene* scene) :
    QGraphicsPathItem(0, scene), m_startControlPoint(), m_endControlPoint()
{
}
 
StateChartGraphEdge::StateChartGraphEdge(Agedge_t* gvGraphEdge,
                                         const QString& transitionName,
                                         QGraphicsScene* scene,
                                         float dpi) :
    QGraphicsPathItem(0, scene),
    m_headNode(gvGraphEdge->head->name),
    m_tailNode(gvGraphEdge->tail->name),
    m_startControlPoint(),
    m_endControlPoint(),
    m_bInfoBoxUnfolded(false),
    m_transitionName(transitionName)
{
    m_InfoPointRadius = dpi / 6;
    m_ArrowScale = dpi / 10;
 
    std::vector<floatPair> controlPoints = calcControlPoints(gvGraphEdge);
    calcPath(controlPoints);
 
    //------------------------------------------------------------------------
 
    //set the path's color, width and arrowhead
    QPen pen;
    pen.setWidthF(1.5);
    pen.setColor(Qt::black);
    this->setPen(pen);
    this->setPath(m_Path);
    m_Arrow = new QGraphicsPolygonItem(this, scene);
    m_Arrow->setBrush(QBrush(Qt::black));
    //calculate the arrowhead's position on basis of second last and last control point
    setArrowHeadPosition(QPointF(controlPoints[int(controlPoints.size()) - 2].first,
                                 controlPoints[int(controlPoints.size()) - 2].second),
                         QPointF(m_endControlPoint.first, m_endControlPoint.second));
 
    //-------------------------------------------------------------------------
 
    //set the transition label
    m_pTransitionLabel = new QGraphicsSimpleTextItem(transitionName, this);
    QRectF transitionLabelRect = m_pTransitionLabel->boundingRect();
    float transitionLength = calcLengthOfLine(m_startControlPoint, m_endControlPoint);
 
    //angle between x direction and edge
    float labelAngle =
        acos((m_endControlPoint.first - m_startControlPoint.first) / transitionLength);
    int arg = 1;
 
    if (m_endControlPoint.second - m_startControlPoint.second < 0)
    {
        labelAngle = -labelAngle; //angle is negative if end is lower than start
    }
 
    //adjust angle if end is to the left of start
    if (m_endControlPoint.first - m_startControlPoint.first < 0)
    {
        labelAngle = pi + labelAngle;
        arg = -1;
    }
 
    //set position and angle of the transition label
    m_pTransitionLabel->setPos(
        m_startControlPoint.first +
            (0.5 * (transitionLength - arg * transitionLabelRect.width()) *
             (m_endControlPoint.first - m_startControlPoint.first) / transitionLength),
        m_startControlPoint.second +
            (0.5 * (transitionLength - arg * transitionLabelRect.width()) *
             (m_endControlPoint.second - m_startControlPoint.second) / transitionLength));
    m_pTransitionLabel->setPos(
        m_Path.pointAtPercent(0.5).x() - arg * transitionLabelRect.width() * 0.5 *
                                             (m_endControlPoint.first - m_startControlPoint.first) /
                                             transitionLength,
        m_Path.pointAtPercent(0.5).y() -
            arg * transitionLabelRect.width() * 0.5 *
                (m_endControlPoint.second - m_startControlPoint.second) / transitionLength);
    m_pTransitionLabel->rotate(labelAngle * 180 / pi);
 
    setInfoPointPosition(
        QPointF(m_pTransitionLabel->pos().x() + transitionLabelRect.width() * cos(labelAngle),
                m_pTransitionLabel->pos().y() + transitionLabelRect.width() * sin(labelAngle)),
        labelAngle);
 
    //-----------------------------------------------------------------------------
 
    m_pTransitionInfoPoint = new QGraphicsEllipseItem(
        this, scene); //how can an ellipse be created from a QGraphicsPathItem (this) ?
    m_Cross = new QGraphicsPathItem(m_pTransitionInfoPoint, scene);
 
    //create info box and text
    m_InfoBox = new QGraphicsRectItem(this);
    m_InfoBox->setBrush(QBrush(Qt::white));
    m_InfoText = new QGraphicsSimpleTextItem(QString("test1\ntest2"), m_InfoBox);
 
    m_InfoText->hide();
    m_InfoBox->hide();
}
 
StateChartGraphEdge::~StateChartGraphEdge()
{
    delete m_Arrow;
    delete m_Cross;
    delete m_InfoText;
    delete m_InfoBox;
    delete m_pTransitionLabel;
    delete m_pTransitionInfoPoint;
}
 
void
StateChartGraphEdge::setTransitionName(const QString& name)
{
    m_transitionName = name;
    m_pTransitionLabel->setText(name);
}
 
void
StateChartGraphEdge::setLineTo(QPointF newEndPoint)
{
    QPointF startPoint(m_startControlPoint.first, m_startControlPoint.second);
    m_Path = QPainterPath(startPoint);
    m_Path.lineTo(newEndPoint);
    m_endControlPoint.first = newEndPoint.x();
    m_endControlPoint.second = newEndPoint.y();
 
    setArrowHeadPosition(QPointF(m_startControlPoint.first, m_startControlPoint.second),
                         QPointF(m_endControlPoint.first, m_endControlPoint.second));
    QRectF transitionRect = m_pTransitionLabel->boundingRect();
    m_pTransitionLabel->resetTransform();
    float transitionLength = sqrt(pow(m_endControlPoint.first - m_startControlPoint.first, 2) +
                                  pow(m_endControlPoint.second - m_startControlPoint.second, 2));
    float labelAngle =
        acos((m_endControlPoint.first - m_startControlPoint.first) / transitionLength);
    int arg = 1;
 
    if (m_endControlPoint.second - m_startControlPoint.second < 0)
    {
        labelAngle = -labelAngle;
    }
 
    if (m_endControlPoint.first - m_startControlPoint.first < 0)
    {
        labelAngle = pi + labelAngle;
        arg = -1;
    }
 
    m_pTransitionLabel->setPos(
        m_startControlPoint.first +
            (0.5 * (transitionLength - arg * transitionRect.width()) *
             (m_endControlPoint.first - m_startControlPoint.first) / transitionLength),
        m_startControlPoint.second +
            (0.5 * (transitionLength - arg * transitionRect.width()) *
             (m_endControlPoint.second - m_startControlPoint.second) / transitionLength));
    m_pTransitionLabel->rotate(labelAngle * 180 / pi);
 
    setInfoPointPosition(
        QPointF(m_pTransitionLabel->pos().x() + transitionRect.width() * cos(labelAngle),
                m_pTransitionLabel->pos().y() + transitionRect.width() * sin(labelAngle)),
        labelAngle);
    this->setPath(m_Path);
}
 
void
StateChartGraphEdge::setLineFrom(QPointF newStartPoint)
{
    QPointF endPoint(m_endControlPoint.first, m_endControlPoint.second);
    m_Path = QPainterPath(newStartPoint);
    m_Path.lineTo(endPoint);
    m_startControlPoint.first = newStartPoint.x();
    m_startControlPoint.second = newStartPoint.y();
    setArrowHeadPosition(QPointF(m_startControlPoint.first, m_startControlPoint.second),
                         QPointF(m_endControlPoint.first, m_endControlPoint.second));
    QRectF transitionRect = m_pTransitionLabel->boundingRect();
    m_pTransitionLabel->resetTransform();
    float transitionLength = sqrt(pow(m_endControlPoint.first - m_startControlPoint.first, 2) +
                                  pow(m_endControlPoint.second - m_startControlPoint.second, 2));
    float labelAngle =
        acos((m_endControlPoint.first - m_startControlPoint.first) / transitionLength);
    int arg = 1;
 
    if (m_endControlPoint.second - m_startControlPoint.second < 0)
    {
        labelAngle = -labelAngle;
    }
 
    if (m_endControlPoint.first - m_startControlPoint.first < 0)
    {
        labelAngle = pi + labelAngle;
        arg = -1;
    }
 
    m_pTransitionLabel->setPos(
        m_startControlPoint.first +
            (0.5 * (transitionLength - arg * transitionRect.width()) *
             (m_endControlPoint.first - m_startControlPoint.first) / transitionLength),
        m_startControlPoint.second +
            (0.5 * (transitionLength - arg * transitionRect.width()) *
             (m_endControlPoint.second - m_startControlPoint.second) / transitionLength));
    m_pTransitionLabel->rotate(labelAngle * 180 / pi);
    setInfoPointPosition(
        QPointF(m_pTransitionLabel->pos().x() + transitionRect.width() * cos(labelAngle),
                m_pTransitionLabel->pos().y() + transitionRect.width() * sin(labelAngle)),
        labelAngle);
    this->setPath(m_Path);
}
 
void
StateChartGraphEdge::setInfoPointPosition(QPointF newPosition, double labelAngle)
{
    QPointF startPoint(0, -m_InfoPointRadius / 3);
    QPointF endPoint(0, m_InfoPointRadius / 3);
    QPointF startPoint2(-m_InfoPointRadius / 3, 0);
    QPointF endPoint2(m_InfoPointRadius / 3, 0);
 
    if (!m_bInfoBoxUnfolded)
    {
        m_CrossPainter = QPainterPath(startPoint);
        m_CrossPainter.lineTo(endPoint);
        m_CrossPainter.moveTo(startPoint2);
        m_CrossPainter.lineTo(endPoint2);
        m_Cross->setPath(m_CrossPainter);
    }
    else
    {
        m_CrossPainter = QPainterPath(startPoint2);
        m_CrossPainter.lineTo(endPoint2);
        m_Cross->setPath(m_CrossPainter);
    }
 
    m_pTransitionInfoPoint->setRect(
        newPosition.x(), newPosition.y() - m_InfoPointRadius, m_InfoPointRadius, m_InfoPointRadius);
    m_Cross->resetTransform();
    m_Cross->setPos(newPosition.x() + m_InfoPointRadius / 2,
                    newPosition.y() - m_InfoPointRadius / 2);
    m_Cross->rotate(labelAngle * 180 / pi);
    m_InfoText->setPos(newPosition.x() + m_InfoPointRadius, newPosition.y() - m_InfoPointRadius);
    m_InfoBox->setRect(newPosition.x() + m_InfoPointRadius,
                       newPosition.y() - m_InfoPointRadius,
                       m_InfoText->boundingRect().width(),
                       m_InfoText->boundingRect().height());
}
 
void
StateChartGraphEdge::setArrowHeadPosition(QPointF headLineSegmentStart, QPointF headLineSegmentEnd)
{
    QPolygonF arrowHead;
    //vector between headLineSegmentStart and headLineSegmentEnd
    QPointF arrowVector = headLineSegmentEnd - headLineSegmentStart;
    //calculate length of arrowVector
    double lengthLastSegment = calcLengthOfLine(headLineSegmentStart, headLineSegmentStart);
    arrowVector.setX(arrowVector.x() / lengthLastSegment);
    arrowVector.setY(arrowVector.y() / lengthLastSegment);
 
    //arrowHead = polygon from headLineSegmentEnd(arrow point ?) to lower "wing" end point to upper "wing" end point
    //why not different order: lower wing - arrow point - upper wing ?
    arrowHead << headLineSegmentEnd;
    arrowHead << QPointF(
        headLineSegmentEnd.x() - m_ArrowScale * (arrowVector.x() - arrowVector.y()),
        headLineSegmentEnd.y() - m_ArrowScale * (arrowVector.y() + arrowVector.x()));
    arrowHead << QPointF(
        headLineSegmentEnd.x() - m_ArrowScale * (arrowVector.x() + arrowVector.y()),
        headLineSegmentEnd.y() - m_ArrowScale * (arrowVector.y() - arrowVector.x()));
 
    //set m_Arrow to arrowHead
    m_Arrow->setPolygon(arrowHead);
}
 
float
StateChartGraphEdge::calcLengthOfLine(floatPair start, floatPair end)
{
    float xDiff = end.first - start.first;
    float yDiff = end.second - start.second;
    return sqrt(pow(xDiff, 2) + pow(yDiff, 2));
}
 
float
StateChartGraphEdge::calcLengthOfLine(QPointF start, QPointF end)
{
    return calcLengthOfLine(floatPair(start.x(), start.y()), floatPair(end.x(), end.y()));
}
 
std::vector<floatPair>
StateChartGraphEdge::calcControlPoints(Agedge_t* gvGraphEdge)
{
    std::vector<floatPair> controlPoints;
 
    //get position of gvGraphEdge as string
    std::string position(agget(gvGraphEdge, const_cast<char*>("pos")));
    size_t oldDividerPos;
    size_t dividerPos;
 
    //dispose of part before first comma
    dividerPos = position.find_first_of(",");
    assert(dividerPos != std::string::npos); //there has to be a comma
    oldDividerPos = dividerPos;
    //    ARMARX_IMPORTANT_S << transitionName.toStdString() <<  " pos: " << strangePos;
 
    //set x position of endControlPoint to the part of position between first and second comma
    dividerPos = position.find_first_of(",", oldDividerPos + 1);
    assert(dividerPos != std::string::npos); //there has to be a comma
    std::string stringEndX = position.substr(oldDividerPos + 1, dividerPos);
    oldDividerPos = dividerPos;
 
    m_endControlPoint.first = boost::lexical_cast<float>(stringEndX);
 
    //set y position of endControlPoint to the part of position between second and third comma
    dividerPos = position.find_first_of(",", oldDividerPos + 1);
    assert(dividerPos != std::string::npos); //there has to be a comma
    std::string stringEndY = position.substr(oldDividerPos + 1, dividerPos);
    oldDividerPos = dividerPos;
 
    m_endControlPoint.second = boost::lexical_cast<float>(stringEndY);
 
    //set x position of startControlPoint to the part of position between third and fourth comma
    dividerPos = position.find_first_of(",", oldDividerPos + 1);
    assert(dividerPos != std::string::npos); //there has to be a comma
    std::string stringStartX = position.substr(oldDividerPos + 1, dividerPos);
    oldDividerPos = dividerPos;
 
    m_startControlPoint.first = boost::lexical_cast<float>(stringStartX);
 
    //set y position of startControlPoint to the part of position between third and fourth comma
    dividerPos = position.find_first_of(",", oldDividerPos + 1);
    std::string stringStartY = position.substr(oldDividerPos + 1, dividerPos);
    oldDividerPos = dividerPos;
 
    m_startControlPoint.second = boost::lexical_cast<float>(stringStartY);
    controlPoints.push_back(m_startControlPoint);
 
    //calculate inner control points
    while (oldDividerPos != std::string::npos)
    {
        floatPair controlPoint;
 
        dividerPos = position.find_first_of(",", oldDividerPos + 1);
        assert(dividerPos !=
               std::string::npos); //there has to be a comma because y position needs to follow
        std::string posX = position.substr(oldDividerPos + 1, dividerPos);
        oldDividerPos = dividerPos;
 
        dividerPos = position.find_first_of(",", oldDividerPos + 1);
        std::string posY = position.substr(oldDividerPos + 1, dividerPos);
        oldDividerPos = dividerPos;
 
        controlPoint.first = boost::lexical_cast<float>(posX);
        controlPoint.second = boost::lexical_cast<float>(posY);
        controlPoints.push_back(controlPoint);
    }
 
    controlPoints.push_back(m_endControlPoint);
 
    return controlPoints;
}
 
void
StateChartGraphEdge::calcPath(std::vector<floatPair> controlPoints)
{
    //construct the path based on the controlPoints
    m_Path.moveTo(controlPoints[0].first, controlPoints[0].second);
 
    if (int(controlPoints.size()) > 2)
    {
        //join all controlpoints by quadratic Bezier curves
        for (int i = 1; i < int(controlPoints.size()) - 1; i += 1)
        {
            m_Path.quadTo(controlPoints[i].first,
                          controlPoints[i].second,
                          controlPoints[i + 1].first,
                          controlPoints[i + 1].second);
            //            m_Path.cubicTo(controlPoints[i].first,controlPoints[i].second,controlPoints[i+1].first,controlPoints[i+1].second,controlPoints[i+2].first,controlPoints[i+2].second);
        }
    }
    else
    {
        //path = line from start to end point
        m_Path.lineTo(controlPoints[controlPoints.size() - 1].first,
                      controlPoints[controlPoints.size() - 1].second);
    }
}
 
QPointF
StateChartGraphEdge::getEndPoint()
{
    QPointF endPoint(m_endControlPoint.first, m_endControlPoint.second);
    return endPoint;
}
 
QPointF
StateChartGraphEdge::getStartPoint()
{
    QPointF startPoint(m_startControlPoint.first, m_startControlPoint.second);
    return startPoint;
}
 
StateChartGraphEdge&
StateChartGraphEdge::operator=(const StateChartGraphEdge&)
{
    return *this;
}
 
void
StateChartGraphEdge::unfoldInfoBox()
{
    m_bInfoBoxUnfolded = true;
    float labelAngle = acos((m_endControlPoint.first - m_startControlPoint.first) /
                            sqrt(pow(m_endControlPoint.first - m_startControlPoint.first, 2) +
                                 pow(m_endControlPoint.second - m_startControlPoint.second, 2)));
 
    if (m_endControlPoint.second - m_startControlPoint.second < 0)
    {
        labelAngle = -labelAngle;
    }
 
    if (m_endControlPoint.first - m_startControlPoint.first < 0)
    {
        labelAngle = pi + labelAngle;
    }
 
    QRectF transitionRect = m_pTransitionLabel->boundingRect();
    setInfoPointPosition(
        QPointF(m_pTransitionLabel->pos().x() + transitionRect.width() * cos(labelAngle),
                m_pTransitionLabel->pos().y() + transitionRect.width() * sin(labelAngle)),
        labelAngle);
    m_InfoText->show();
    m_InfoBox->show();
}
 
void
StateChartGraphEdge::foldInfoBox()
{
    m_bInfoBoxUnfolded = false;
    float labelAngle = acos((m_endControlPoint.first - m_startControlPoint.first) /
                            sqrt(pow(m_endControlPoint.first - m_startControlPoint.first, 2) +
                                 pow(m_endControlPoint.second - m_startControlPoint.second, 2)));
 
    if (m_endControlPoint.second - m_startControlPoint.second < 0)
    {
        labelAngle = -labelAngle;
    }
 
    if (m_endControlPoint.first - m_startControlPoint.first < 0)
    {
        labelAngle = pi + labelAngle;
    }
 
    QRectF transitionRect = m_pTransitionLabel->boundingRect();
    setInfoPointPosition(
        QPointF(m_pTransitionLabel->pos().x() + transitionRect.width() * cos(labelAngle),
                m_pTransitionLabel->pos().y() + transitionRect.width() * sin(labelAngle)),
        labelAngle);
    m_InfoText->hide();
    m_InfoBox->hide();
}
 
bool
StateChartGraphEdge::infoBoxClicked(const QPointF& point)
{
    bool infoBoxClicked = m_pTransitionInfoPoint->contains(point);
 
    if (infoBoxClicked)
    {
        if (!m_bInfoBoxUnfolded)
        {
            unfoldInfoBox();
        }
        else
        {
            foldInfoBox();
        }
    }
 
    return infoBoxClicked;
}