GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/gui/dialog/configuration.cc Lines: 2 110 1.8 %
Date: 2020-05-14 11:23:33 Branches: 2 665 0.3 %

Line Branch Exec Source
1
// Copyright (c) 2015-2018, LAAS-CNRS
2
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3
//
4
// This file is part of gepetto-viewer.
5
// gepetto-viewer is free software: you can redistribute it
6
// and/or modify it under the terms of the GNU Lesser General Public
7
// License as published by the Free Software Foundation, either version
8
// 3 of the License, or (at your option) any later version.
9
//
10
// gepetto-viewer is distributed in the hope that it will be
11
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13
// General Lesser Public License for more details. You should have
14
// received a copy of the GNU Lesser General Public License along with
15
// gepetto-viewer. If not, see <http://www.gnu.org/licenses/>.
16
17
#include <gepetto/gui/dialog/configuration.hh>
18
19
#include <QtGlobal>
20
#include <QDialog>
21
#include <QDialogButtonBox>
22
#include <QDoubleSpinBox>
23
#include <QFormLayout>
24
#include <QLabel>
25
#include <QLineEdit>
26
#include <QPushButton>
27
28
#include <gepetto/viewer/node-property.h>
29
30
namespace gepetto {
31
  namespace gui {
32
    using viewer::Configuration;
33
34
    void setSpinBoxRange(const viewer::Property* prop, QDoubleSpinBox* sb)
35
    {
36
      sb->setRange(-std::numeric_limits<double>::max(), std::numeric_limits<double>::max());
37
      const viewer::Range<float>* range = dynamic_cast<const viewer::Range<float>*>(prop);
38
      if (range) {
39
        if (range->hasMin()) sb->setMinimum(static_cast<double>(range->min));
40
        if (range->hasMax()) sb->setMaximum(static_cast<double>(range->max));
41
        sb->setSingleStep(static_cast<double>(range->step));
42
#if QT_VERSION > QT_VERSION_CHECK(5, 12, 0)
43
        if (range->adaptiveDecimal) sb->setStepType (QAbstractSpinBox::AdaptiveDecimalStepType);
44
#endif
45
      }
46
    }
47
48
    void getEulerFromQuat(osg::Quat q, double& heading, double& attitude, double& bank)
49
    {
50
      double limit = 0.499999;
51
52
      double sqx = q.x()*q.x();
53
      double sqy = q.y()*q.y();
54
      double sqz = q.z()*q.z();
55
56
      double t = q.x()*q.y() + q.z()*q.w();
57
58
      if (t>limit) { // gimbal lock ?
59
        heading = 2 * atan2(q.x(),q.w());
60
        attitude = osg::PI_2;
61
        bank = 0;
62
      } else if (t<-limit) {
63
        heading = -2 * atan2(q.x(),q.w());
64
        attitude = - osg::PI_2;
65
        bank = 0;
66
      } else {
67
        heading = atan2(2*q.y()*q.w()-2*q.x()*q.z() , 1 - 2*sqy - 2*sqz);
68
        attitude = asin(2*t);
69
        bank = atan2(2*q.x()*q.w()-2*q.y()*q.z() , 1 - 2*sqx - 2*sqz);
70
      }
71
    }
72
73
    void getQuatFromEuler(double heading, double attitude, double bank, osg::Quat& q)
74
    {
75
      double c1 = cos(heading/2);
76
      double s1 = sin(heading/2);
77
      double c2 = cos(attitude/2);
78
      double s2 = sin(attitude/2);
79
      double c3 = cos(bank/2);
80
      double s3 = sin(bank/2);
81
      double c1c2 = c1*c2;
82
      double s1s2 = s1*s2;
83
84
      double w =c1c2*c3 - s1s2*s3;
85
      double x =c1c2*s3 + s1s2*c3;
86
      double y =s1*c2*c3 + c1*s2*s3;
87
      double z =c1*s2*c3 - s1*c2*s3;
88
89
      q[0] = x; q[1] = y; q[2] = z; q[3] = w;
90
    }
91
92
    template <typename Vector>
93
    inline void getValues (Vector& v, const QVector<QDoubleSpinBox*> spinBoxes)
94
    {
95
      int i = 0;
96
      foreach (QDoubleSpinBox* sb, spinBoxes) {
97
        v[i] = (float)sb->value ();
98
        ++i;
99
      }
100
    }
101
102
    inline void getValues (Configuration& cfg, const QVector<QDoubleSpinBox*> spinBoxes)
103
    {
104
      const QDoubleSpinBox *x = spinBoxes[0], *y = spinBoxes[1], *z = spinBoxes[2],
105
        *roll = spinBoxes[3], *pitch = spinBoxes[4], *yaw = spinBoxes[5];
106
107
      cfg.position[0] = (float)x->value();
108
      cfg.position[1] = (float)y->value();
109
      cfg.position[2] = (float)z->value();
110
      getQuatFromEuler (pitch->value(), roll->value(), yaw->value(), cfg.quat);
111
    }
112
113
    template <typename Vector>
114
    inline void setValues (const Vector& v, QVector<QDoubleSpinBox*> spinBoxes)
115
    {
116
      int i = 0;
117
      foreach (QDoubleSpinBox* sb, spinBoxes) {
118
        sb->setValue (v[i]);
119
        ++i;
120
      }
121
    }
122
123
    inline void setValues (const Configuration& cfg, QVector<QDoubleSpinBox*> spinBoxes)
124
    {
125
      QDoubleSpinBox *x = spinBoxes[0], *y = spinBoxes[1], *z = spinBoxes[2],
126
        *roll = spinBoxes[3], *pitch = spinBoxes[4], *yaw = spinBoxes[5];
127
128
      double _r,_p,_y;
129
      getEulerFromQuat (cfg.quat,_p,_y,_r);
130
131
      x->setValue(cfg.position[0]);
132
      y->setValue(cfg.position[1]);
133
      z->setValue(cfg.position[2]);
134
135
      roll ->setValue(_r);
136
      pitch->setValue(_p);
137
      yaw  ->setValue(_y);
138
    }
139
140
1
    static const QString sep(", ");
141
142
    template <typename Vector>
143
    QString valuesToString (const QVector<QDoubleSpinBox*> spinBoxes)
144
    {
145
      QString text = "[ ";
146
      foreach (const QDoubleSpinBox* sb, spinBoxes)
147
        text += QString::number (sb->value()) + sep;
148
      text += " ]";
149
      return text;
150
    }
151
152
    template <>
153
    QString valuesToString<Configuration> (const QVector<QDoubleSpinBox*> spinBoxes)
154
    {
155
      const QDoubleSpinBox *x = spinBoxes[0], *y = spinBoxes[1], *z = spinBoxes[2],
156
        *roll = spinBoxes[3], *pitch = spinBoxes[4], *yaw = spinBoxes[5];
157
158
      osg::Quat quat;
159
      getQuatFromEuler (pitch->value(), roll->value(), yaw->value(), quat);
160
161
      return "[ "
162
          + QString::number (x->value()) + sep
163
          + QString::number (y->value()) + sep
164
          + QString::number (z->value()) + sep
165
          + QString::number (quat.x()) + sep
166
          + QString::number (quat.y()) + sep
167
          + QString::number (quat.z()) + sep
168
          + QString::number (quat.w()) + " ]";
169
    }
170
171
#define DEFINE_PROPERTY_EDITOR(Name,Type)                                      \
172
    void Name::updateValue ()                                                  \
173
    {                                                                          \
174
      Type cfg;                                                                \
175
      getValues(cfg, spinBoxes);                                               \
176
      setPyValue ();                                                           \
177
      emit valueChanged (cfg);                                                 \
178
    }                                                                          \
179
                                                                               \
180
    void Name::setValueFromProperty (viewer::Property* prop)                   \
181
    {                                                                          \
182
      Type cfg;                                                                \
183
      prop->get (cfg);                                                         \
184
      setValues(cfg, spinBoxes);                                               \
185
      setPyValue();                                                            \
186
    }                                                                          \
187
                                                                               \
188
    void Name::set (const Type& v)                                             \
189
    {                                                                          \
190
      foreach (QDoubleSpinBox* sb, spinBoxes)                                  \
191
        sb->blockSignals(true);                                                \
192
      setValues(v, spinBoxes);                                                 \
193
      foreach (QDoubleSpinBox* sb, spinBoxes)                                  \
194
        sb->blockSignals(false);                                               \
195
    }                                                                          \
196
                                                                               \
197
    void Name::setPyValue ()                                                   \
198
    {                                                                          \
199
      pyValue->setText (valuesToString<Type>(spinBoxes));                      \
200
    }
201
202
    DEFINE_PROPERTY_EDITOR(Vector2Dialog, osgVector2)
203
    DEFINE_PROPERTY_EDITOR(Vector3Dialog, osgVector3)
204
    DEFINE_PROPERTY_EDITOR(Vector4Dialog, osgVector4)
205
    DEFINE_PROPERTY_EDITOR(ConfigurationDialog, Configuration)
206
207
    template<typename T>
208
    QString rowLabel(int i) { return "X" + QString::number(i); }
209
210
    template<>
211
    QString rowLabel<Configuration>(int i)
212
    {
213
      switch(i)
214
      {
215
        case 0: return "X";
216
        case 1: return "Y";
217
        case 2: return "Z";
218
        case 3: return "Roll";
219
        case 4: return "Pitch";
220
        case 5: return "Yaw";
221
      }
222
      abort();
223
    }
224
225
    template<typename Dialog>
226
    void constructor_hook(Dialog*, QVector<QDoubleSpinBox*>) {}
227
228
    void constructor_hook(ConfigurationDialog*, QVector<QDoubleSpinBox*> spinBoxes)
229
    {
230
      foreach (QDoubleSpinBox* sb, spinBoxes) {
231
        sb->setSingleStep(0.01);
232
        sb->setDecimals(4);
233
      }
234
      foreach (QDoubleSpinBox* sb, spinBoxes.mid(3))
235
        sb->setRange(-osg::PI, osg::PI);
236
    }
237
238
#define DEFINE_PROPERTY_EDITOR_CONSTRUCTOR(Name,Type,N)                        \
239
    Name::Name (viewer::Property* property, QWidget *parent)                   \
240
      : QDialog (parent)                                                       \
241
      , pyValue (new QLineEdit)                                                \
242
    {                                                                          \
243
      setModal (false);                                                        \
244
      pyValue->setReadOnly (true);                                             \
245
                                                                               \
246
      QFormLayout *layout = new QFormLayout;                                   \
247
      layout->addRow(new QLabel (property->objectName()));                     \
248
                                                                               \
249
      spinBoxes.reserve(N);                                                    \
250
      for (int i = 0; i < N; ++i) {                                            \
251
        QDoubleSpinBox* sb (new QDoubleSpinBox);                               \
252
        spinBoxes.append(sb);                                                  \
253
        setSpinBoxRange(property, sb);                                         \
254
        connect(sb, SIGNAL(valueChanged(double)), SLOT(updateValue()));        \
255
                                                                               \
256
        layout->addRow(rowLabel<Type>(i), sb);                                 \
257
      }                                                                        \
258
      layout->addRow("value", pyValue);                                        \
259
                                                                               \
260
      QDialogButtonBox *buttonBox = new QDialogButtonBox (                     \
261
          QDialogButtonBox::Close, Qt::Horizontal);                            \
262
      connect (buttonBox->button (QDialogButtonBox::Close),                    \
263
          SIGNAL(clicked(bool)), SLOT(accept()));                              \
264
                                                                               \
265
      layout->addRow(buttonBox);                                               \
266
                                                                               \
267
      setLayout (layout);                                                      \
268
                                                                               \
269
      constructor_hook(this, spinBoxes);                                       \
270
    }
271
272
    DEFINE_PROPERTY_EDITOR_CONSTRUCTOR(Vector2Dialog, osgVector2, 2)
273
    DEFINE_PROPERTY_EDITOR_CONSTRUCTOR(Vector3Dialog, osgVector3, 3)
274
    DEFINE_PROPERTY_EDITOR_CONSTRUCTOR(Vector4Dialog, osgVector4, 4)
275
    DEFINE_PROPERTY_EDITOR_CONSTRUCTOR(ConfigurationDialog, Configuration, 6)
276
  } // namespace gui
277

3
} // namespace gepetto