Matrix4fWidgets.h

Go to the documentation of this file.

#pragma once
 
#include <QDoubleSpinBox>
#include <QHBoxLayout>
#include <QLabel>
#include <QWidget>
 
#include "Basic.h"
 
namespace armarx::RemoteGui
{
    struct PosRPYSpinBoxesHandler : TypedWidget<PosRPYSpinBoxes, QWidget, VALUE_VARIANT_MATRIX4>
    {
        static bool
        isValid(RemoteWidgetT const& desc, std::ostream& out)
        {
#define cmp_or_log(l, c, r)                                                                        \
    (l c r ? true                                                                                  \
           : (out << " FAILED: " #l " " #c " " #r " " << VAROUT(l) << ", " << VAROUT(r) << '\n',   \
              false))
 
            return cmp_or_log(desc.minPos.x, <=, desc.maxPos.x) &&
                   cmp_or_log(desc.minPos.y, <=, desc.maxPos.y) &&
                   cmp_or_log(desc.minPos.z, <=, desc.maxPos.z) &&
                   cmp_or_log(desc.stepsPos.x, >, 0) && cmp_or_log(desc.stepsPos.y, >, 0) &&
                   cmp_or_log(desc.stepsPos.z, >, 0) && cmp_or_log(desc.decimalsPos.x, >=, 0) &&
                   cmp_or_log(desc.decimalsPos.y, >=, 0) && cmp_or_log(desc.decimalsPos.z, >=, 0)
 
                   && cmp_or_log(desc.minRPY.x, <=, desc.maxRPY.x) &&
                   cmp_or_log(desc.minRPY.y, <=, desc.maxRPY.y) &&
                   cmp_or_log(desc.minRPY.z, <=, desc.maxRPY.z) &&
                   cmp_or_log(desc.stepsRPY.x, >, 0) && cmp_or_log(desc.stepsRPY.y, >, 0) &&
                   cmp_or_log(desc.stepsRPY.z, >, 0) && cmp_or_log(desc.decimalsRPY.x, >=, 0) &&
                   cmp_or_log(desc.decimalsRPY.y, >=, 0) && cmp_or_log(desc.decimalsRPY.z, >=, 0);
#undef cmp_or_log
        }
 
        static QWidgetT*
        createWidget(RemoteWidgetT const& desc,
                     CreateWidgetCallback const& createChild,
                     const QObject* stateChangeReceiver,
                     const char* stateChangeSlot)
        {
            ARMARX_TRACE;
            QWidget* widget = new QWidget;
            QHBoxLayout* l = new QHBoxLayout;
            l->setContentsMargins(0, 0, 0, 0);
            widget->setLayout(l);
            widget->setToolTip(QString::fromStdString(desc.toolTip));
 
            Eigen::Vector3f minPos = fromIce(desc.minPos);
            Eigen::Vector3f maxPos = fromIce(desc.maxPos);
            Eigen::Vector3i decimalsPos = fromIce(desc.decimalsPos);
            Eigen::Vector3i stepsPos = fromIce(desc.stepsPos);
            for (int i = 0; i < 3; ++i)
            {
                QDoubleSpinBox* e = new QDoubleSpinBox;
                l->addWidget(e);
                e->setMinimum(minPos(i));
                e->setMaximum(maxPos(i));
                e->setDecimals(decimalsPos(i));
                e->setSingleStep((maxPos(i) - minPos(i)) / stepsPos(i));
                QObject::connect(
                    e, SIGNAL(valueChanged(double)), stateChangeReceiver, stateChangeSlot);
            }
            l->addWidget(new QLabel{"/"});
 
            Eigen::Vector3f minRPY = fromIce(desc.minRPY);
            Eigen::Vector3f maxRPY = fromIce(desc.maxRPY);
            Eigen::Vector3i decimalsRPY = fromIce(desc.decimalsRPY);
            Eigen::Vector3i stepsRPY = fromIce(desc.stepsRPY);
            for (int i = 0; i < 3; ++i)
            {
                QDoubleSpinBox* e = new QDoubleSpinBox;
                l->addWidget(e);
                e->setMinimum(minRPY(i));
                e->setMaximum(maxRPY(i));
                e->setDecimals(decimalsRPY(i));
                e->setSingleStep((maxRPY(i) - minRPY(i)) / stepsRPY(i));
                QObject::connect(
                    e, SIGNAL(valueChanged(double)), stateChangeReceiver, stateChangeSlot);
            }
            return widget;
        }
 
        static void
        updateGui(RemoteWidgetT const&, QWidgetT* widget, ValueT const& m)
        {
            float rx, ry, rz;
            ry = std::atan2(-m(2, 0), sqrtf(m(0, 0) * m(0, 0) + m(1, 0) * m(1, 0)));
 
            if (fabs(ry - static_cast<float>(M_PI) * 0.5f) < 1e-10f)
            {
                rx = 0;
                rz = std::atan2(m(0, 1), m(1, 1));
            }
            else if (std::fabs(ry + static_cast<float>(M_PI) * 0.5f) < 1e-10f)
            {
                rx = 0;
                rz = -std::atan2(m(0, 1), m(1, 1));
            }
            else
            {
                float cb = 1.0f / std::cos(ry);
                rx = std::atan2(m(2, 1) * cb, m(2, 2) * cb);
                rz = std::atan2(m(1, 0) * cb, m(0, 0) * cb);
            }
 
            static_cast<QDoubleSpinBox*>(widget->layout()->itemAt(0)->widget())->setValue(m(0, 3));
            static_cast<QDoubleSpinBox*>(widget->layout()->itemAt(1)->widget())->setValue(m(1, 3));
            static_cast<QDoubleSpinBox*>(widget->layout()->itemAt(2)->widget())->setValue(m(2, 3));
 
            static_cast<QDoubleSpinBox*>(widget->layout()->itemAt(4)->widget())->setValue(rx);
            static_cast<QDoubleSpinBox*>(widget->layout()->itemAt(5)->widget())->setValue(ry);
            static_cast<QDoubleSpinBox*>(widget->layout()->itemAt(6)->widget())->setValue(rz);
        }
 
        static ValueT
        handleGuiChange(RemoteWidgetT const&, QWidgetT* widget)
        {
            QWidgetT* parent = static_cast<QWidget*>(widget->parent());
 
            Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
 
            const float tx = static_cast<float>(
                static_cast<QDoubleSpinBox*>(parent->layout()->itemAt(0)->widget())->value());
            const float ty = static_cast<float>(
                static_cast<QDoubleSpinBox*>(parent->layout()->itemAt(1)->widget())->value());
            const float tz = static_cast<float>(
                static_cast<QDoubleSpinBox*>(parent->layout()->itemAt(2)->widget())->value());
            const float rx = static_cast<float>(
                static_cast<QDoubleSpinBox*>(parent->layout()->itemAt(4)->widget())->value());
            const float ry = static_cast<float>(
                static_cast<QDoubleSpinBox*>(parent->layout()->itemAt(5)->widget())->value());
            const float rz = static_cast<float>(
                static_cast<QDoubleSpinBox*>(parent->layout()->itemAt(6)->widget())->value());
 
            const float sgamma = sinf(rx);
            const float cgamma = cosf(rx);
            const float sbeta = sinf(ry);
            const float cbeta = cosf(ry);
            const float salpha = sinf(rz);
            const float calpha = cosf(rz);
 
            m(0, 0) = calpha * cbeta;
            m(0, 1) = calpha * sbeta * sgamma - salpha * cgamma;
            m(0, 2) = calpha * sbeta * cgamma + salpha * sgamma;
            m(0, 3) = tx;
 
            m(1, 0) = salpha * cbeta;
            m(1, 1) = salpha * sbeta * sgamma + calpha * cgamma;
            m(1, 2) = salpha * sbeta * cgamma - calpha * sgamma;
            m(1, 3) = ty;
 
            m(2, 0) = -sbeta;
            m(2, 1) = cbeta * sgamma;
            m(2, 2) = cbeta * cgamma;
            m(2, 3) = tz;
 
            return m;
        }
    };
} // namespace armarx::RemoteGui