Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/gui/dialog/configuration.cc	Lines:	0	121	0.0 %
Date:	2024-04-14 11:13:22	Branches:	0	529	0.0 %


// Copyright (c) 2015-2018, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of gepetto-viewer.
// gepetto-viewer is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// gepetto-viewer 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 Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// gepetto-viewer. If not, see <http://www.gnu.org/licenses/>.

#include <gepetto/viewer/node-property.h>

#include <QDialog>
#include <QDialogButtonBox>
#include <QDoubleSpinBox>
#include <QFormLayout>
#include <QLabel>
#include <QLineEdit>
#include <QPushButton>
#include <QtGlobal>
#include <gepetto/gui/dialog/configuration.hh>

namespace gepetto {
namespace gui {
using viewer::Configuration;

void setSpinBoxRange(const viewer::Property* prop, QDoubleSpinBox* sb) {
  sb->setRange(-std::numeric_limits<double>::max(),
               std::numeric_limits<double>::max());
  const viewer::Range<float>* range =
      dynamic_cast<const viewer::Range<float>*>(prop);
  if (range) {
    if (range->hasMin()) sb->setMinimum(static_cast<double>(range->min));
    if (range->hasMax()) sb->setMaximum(static_cast<double>(range->max));
    sb->setSingleStep(static_cast<double>(range->step));
#if QT_VERSION > QT_VERSION_CHECK(5, 12, 0)
    if (range->adaptiveDecimal)
      sb->setStepType(QAbstractSpinBox::AdaptiveDecimalStepType);
#endif
  }
}

void getEulerFromQuat(osg::Quat q, double& heading, double& attitude,
                      double& bank) {
  double limit = 0.499999;

  double sqx = q.x() * q.x();
  double sqy = q.y() * q.y();
  double sqz = q.z() * q.z();

  double t = q.x() * q.y() + q.z() * q.w();

  if (t > limit) {  // gimbal lock ?
    heading = 2 * atan2(q.x(), q.w());
    attitude = osg::PI_2;
    bank = 0;
  } else if (t < -limit) {
    heading = -2 * atan2(q.x(), q.w());
    attitude = -osg::PI_2;
    bank = 0;
  } else {
    heading =
        atan2(2 * q.y() * q.w() - 2 * q.x() * q.z(), 1 - 2 * sqy - 2 * sqz);
    attitude = asin(2 * t);
    bank = atan2(2 * q.x() * q.w() - 2 * q.y() * q.z(), 1 - 2 * sqx - 2 * sqz);
  }
}

void getQuatFromEuler(double heading, double attitude, double bank,
                      osg::Quat& q) {
  double c1 = cos(heading / 2);
  double s1 = sin(heading / 2);
  double c2 = cos(attitude / 2);
  double s2 = sin(attitude / 2);
  double c3 = cos(bank / 2);
  double s3 = sin(bank / 2);
  double c1c2 = c1 * c2;
  double s1s2 = s1 * s2;

  double w = c1c2 * c3 - s1s2 * s3;
  double x = c1c2 * s3 + s1s2 * c3;
  double y = s1 * c2 * c3 + c1 * s2 * s3;
  double z = c1 * s2 * c3 - s1 * c2 * s3;

  q[0] = x;
  q[1] = y;
  q[2] = z;
  q[3] = w;
}

template <typename Vector>
inline void getValues(Vector& v, const QVector<QDoubleSpinBox*> spinBoxes) {
  int i = 0;
  foreach (QDoubleSpinBox* sb, spinBoxes) {
    v[i] = (float)sb->value();
    ++i;
  }
}

inline void getValues(Configuration& cfg,
                      const QVector<QDoubleSpinBox*> spinBoxes) {
  const QDoubleSpinBox *x = spinBoxes[0], *y = spinBoxes[1], *z = spinBoxes[2],
                       *roll = spinBoxes[3], *pitch = spinBoxes[4],
                       *yaw = spinBoxes[5];

  cfg.position[0] = (float)x->value();
  cfg.position[1] = (float)y->value();
  cfg.position[2] = (float)z->value();
  getQuatFromEuler(pitch->value(), roll->value(), yaw->value(), cfg.quat);
}

template <typename Vector>
inline void setValues(const Vector& v, QVector<QDoubleSpinBox*> spinBoxes) {
  int i = 0;
  foreach (QDoubleSpinBox* sb, spinBoxes) {
    sb->setValue(v[i]);
    ++i;
  }
}

inline void setValues(const Configuration& cfg,
                      QVector<QDoubleSpinBox*> spinBoxes) {
  QDoubleSpinBox *x = spinBoxes[0], *y = spinBoxes[1], *z = spinBoxes[2],
                 *roll = spinBoxes[3], *pitch = spinBoxes[4],
                 *yaw = spinBoxes[5];

  double _r, _p, _y;
  getEulerFromQuat(cfg.quat, _p, _r, _y);

  x->setValue(cfg.position[0]);
  y->setValue(cfg.position[1]);
  z->setValue(cfg.position[2]);

  roll->setValue(_r);
  pitch->setValue(_p);
  yaw->setValue(_y);
}

static const QString sep(", ");

template <typename Vector>
QString valuesToString(const QVector<QDoubleSpinBox*> spinBoxes) {
  QString text = "[ ";
  foreach (const QDoubleSpinBox* sb, spinBoxes)
    text += QString::number(sb->value()) + sep;
  text += " ]";
  return text;
}

template <>
QString valuesToString<Configuration>(
    const QVector<QDoubleSpinBox*> spinBoxes) {
  const QDoubleSpinBox *x = spinBoxes[0], *y = spinBoxes[1], *z = spinBoxes[2],
                       *roll = spinBoxes[3], *pitch = spinBoxes[4],
                       *yaw = spinBoxes[5];

  osg::Quat quat;
  getQuatFromEuler(pitch->value(), roll->value(), yaw->value(), quat);

  return "[ " + QString::number(x->value()) + sep +
         QString::number(y->value()) + sep + QString::number(z->value()) + sep +
         QString::number(quat.x()) + sep + QString::number(quat.y()) + sep +
         QString::number(quat.z()) + sep + QString::number(quat.w()) + " ]";
}

#define DEFINE_PROPERTY_EDITOR(Name, Type)                           \
  void Name::updateValue() {                                         \
    Type cfg;                                                        \
    getValues(cfg, spinBoxes);                                       \
    setPyValue();                                                    \
    emit valueChanged(cfg);                                          \
  }                                                                  \
                                                                     \
  void Name::setValueFromProperty(viewer::Property* prop) {          \
    Type cfg;                                                        \
    prop->get(cfg);                                                  \
    setValues(cfg, spinBoxes);                                       \
    setPyValue();                                                    \
  }                                                                  \
                                                                     \
  void Name::set(const Type& v) {                                    \
    foreach (QDoubleSpinBox* sb, spinBoxes) sb->blockSignals(true);  \
    setValues(v, spinBoxes);                                         \
    foreach (QDoubleSpinBox* sb, spinBoxes) sb->blockSignals(false); \
  }                                                                  \
                                                                     \
  void Name::setPyValue() { pyValue->setText(valuesToString<Type>(spinBoxes)); }

DEFINE_PROPERTY_EDITOR(Vector2Dialog, osgVector2)
DEFINE_PROPERTY_EDITOR(Vector3Dialog, osgVector3)
DEFINE_PROPERTY_EDITOR(Vector4Dialog, osgVector4)
DEFINE_PROPERTY_EDITOR(ConfigurationDialog, Configuration)

template <typename T>
QString rowLabel(int i) {
  return "X" + QString::number(i);
}

template <>
QString rowLabel<Configuration>(int i) {
  switch (i) {
    case 0:
      return "X";
    case 1:
      return "Y";
    case 2:
      return "Z";
    case 3:
      return "Roll";
    case 4:
      return "Pitch";
    case 5:
      return "Yaw";
  }
  abort();
}

template <typename Dialog>
void constructor_hook(Dialog*, QVector<QDoubleSpinBox*>) {}

void constructor_hook(ConfigurationDialog*,
                      QVector<QDoubleSpinBox*> spinBoxes) {
  foreach (QDoubleSpinBox* sb, spinBoxes) {
    sb->setSingleStep(0.01);
    sb->setDecimals(4);
  }
  foreach (QDoubleSpinBox* sb, spinBoxes.mid(3))
    sb->setRange(-osg::PI, osg::PI);
}

#define DEFINE_PROPERTY_EDITOR_CONSTRUCTOR(Name, Type, N)                      \
  Name::Name(viewer::Property* property, QWidget* parent)                      \
      : QDialog(parent), pyValue(new QLineEdit) {                              \
    setModal(false);                                                           \
    pyValue->setReadOnly(true);                                                \
                                                                               \
    QFormLayout* layout = new QFormLayout;                                     \
    layout->addRow(new QLabel(property->objectName()));                        \
                                                                               \
    spinBoxes.reserve(N);                                                      \
    for (int i = 0; i < N; ++i) {                                              \
      QDoubleSpinBox* sb(new QDoubleSpinBox);                                  \
      spinBoxes.append(sb);                                                    \
      setSpinBoxRange(property, sb);                                           \
      connect(sb, SIGNAL(valueChanged(double)), SLOT(updateValue()));          \
                                                                               \
      layout->addRow(rowLabel<Type>(i), sb);                                   \
    }                                                                          \
    layout->addRow("value", pyValue);                                          \
                                                                               \
    QDialogButtonBox* buttonBox =                                              \
        new QDialogButtonBox(QDialogButtonBox::Close, Qt::Horizontal);         \
    connect(buttonBox->button(QDialogButtonBox::Close), SIGNAL(clicked(bool)), \
            SLOT(accept()));                                                   \
                                                                               \
    layout->addRow(buttonBox);                                                 \
                                                                               \
    setLayout(layout);                                                         \
                                                                               \
    constructor_hook(this, spinBoxes);                                         \
  }

DEFINE_PROPERTY_EDITOR_CONSTRUCTOR(Vector2Dialog, osgVector2, 2)
DEFINE_PROPERTY_EDITOR_CONSTRUCTOR(Vector3Dialog, osgVector3, 3)
DEFINE_PROPERTY_EDITOR_CONSTRUCTOR(Vector4Dialog, osgVector4, 4)
DEFINE_PROPERTY_EDITOR_CONSTRUCTOR(ConfigurationDialog, Configuration, 6)
}  // namespace gui
}  // namespace gepetto


Generated by: GCOVR (Version 4.2)

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/viewer/node-property.h>
18
19			#include <QDialog>
20			#include <QDialogButtonBox>
21			#include <QDoubleSpinBox>
22			#include <QFormLayout>
23			#include <QLabel>
24			#include <QLineEdit>
25			#include <QPushButton>
26			#include <QtGlobal>
27			#include <gepetto/gui/dialog/configuration.hh>
28
29			namespace gepetto {
30			namespace gui {
31			using viewer::Configuration;
32
33			void setSpinBoxRange(const viewer::Property* prop, QDoubleSpinBox* sb) {
34			sb->setRange(-std::numeric_limits<double>::max(),
35			std::numeric_limits<double>::max());
36			const viewer::Range<float>* range =
37			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)
44			sb->setStepType(QAbstractSpinBox::AdaptiveDecimalStepType);
45			#endif
46			}
47			}
48
49			void getEulerFromQuat(osg::Quat q, double& heading, double& attitude,
50			double& bank) {
51			double limit = 0.499999;
52
53			double sqx = q.x() * q.x();
54			double sqy = q.y() * q.y();
55			double sqz = q.z() * q.z();
56
57			double t = q.x() * q.y() + q.z() * q.w();
58
59			if (t > limit) { // gimbal lock ?
60			heading = 2 * atan2(q.x(), q.w());
61			attitude = osg::PI_2;
62			bank = 0;
63			} else if (t < -limit) {
64			heading = -2 * atan2(q.x(), q.w());
65			attitude = -osg::PI_2;
66			bank = 0;
67			} else {
68			heading =
69			atan2(2 * q.y() * q.w() - 2 * q.x() * q.z(), 1 - 2 * sqy - 2 * sqz);
70			attitude = asin(2 * t);
71			bank = atan2(2 * q.x() * q.w() - 2 * q.y() * q.z(), 1 - 2 * sqx - 2 * sqz);
72			}
73			}
74
75			void getQuatFromEuler(double heading, double attitude, double bank,
76			osg::Quat& q) {
77			double c1 = cos(heading / 2);
78			double s1 = sin(heading / 2);
79			double c2 = cos(attitude / 2);
80			double s2 = sin(attitude / 2);
81			double c3 = cos(bank / 2);
82			double s3 = sin(bank / 2);
83			double c1c2 = c1 * c2;
84			double s1s2 = s1 * s2;
85
86			double w = c1c2 * c3 - s1s2 * s3;
87			double x = c1c2 * s3 + s1s2 * c3;
88			double y = s1 * c2 * c3 + c1 * s2 * s3;
89			double z = c1 * s2 * c3 - s1 * c2 * s3;
90
91			q[0] = x;
92			q[1] = y;
93			q[2] = z;
94			q[3] = w;
95			}
96
97			template <typename Vector>
98			inline void getValues(Vector& v, const QVector<QDoubleSpinBox*> spinBoxes) {
99			int i = 0;
100			foreach (QDoubleSpinBox* sb, spinBoxes) {
101			v[i] = (float)sb->value();
102			++i;
103			}
104			}
105
106			inline void getValues(Configuration& cfg,
107			const QVector<QDoubleSpinBox*> spinBoxes) {
108			const QDoubleSpinBox x = spinBoxes[0], y = spinBoxes[1], *z = spinBoxes[2],
109			roll = spinBoxes[3], pitch = spinBoxes[4],
110			*yaw = spinBoxes[5];
111
112			cfg.position[0] = (float)x->value();
113			cfg.position[1] = (float)y->value();
114			cfg.position[2] = (float)z->value();
115			getQuatFromEuler(pitch->value(), roll->value(), yaw->value(), cfg.quat);
116			}
117
118			template <typename Vector>
119			inline void setValues(const Vector& v, QVector<QDoubleSpinBox*> spinBoxes) {
120			int i = 0;
121			foreach (QDoubleSpinBox* sb, spinBoxes) {
122			sb->setValue(v[i]);
123			++i;
124			}
125			}
126
127			inline void setValues(const Configuration& cfg,
128			QVector<QDoubleSpinBox*> spinBoxes) {
129			QDoubleSpinBox x = spinBoxes[0], y = spinBoxes[1], *z = spinBoxes[2],
130			roll = spinBoxes[3], pitch = spinBoxes[4],
131			*yaw = spinBoxes[5];
132
133			double _r, _p, _y;
134			getEulerFromQuat(cfg.quat, _p, _r, _y);
135
136			x->setValue(cfg.position[0]);
137			y->setValue(cfg.position[1]);
138			z->setValue(cfg.position[2]);
139
140			roll->setValue(_r);
141			pitch->setValue(_p);
142			yaw->setValue(_y);
143			}
144
145			static const QString sep(", ");
146
147			template <typename Vector>
148			QString valuesToString(const QVector<QDoubleSpinBox*> spinBoxes) {
149			QString text = "[ ";
150			foreach (const QDoubleSpinBox* sb, spinBoxes)
151			text += QString::number(sb->value()) + sep;
152			text += " ]";
153			return text;
154			}
155
156			template <>
157			QString valuesToString<Configuration>(
158			const QVector<QDoubleSpinBox*> spinBoxes) {
159			const QDoubleSpinBox x = spinBoxes[0], y = spinBoxes[1], *z = spinBoxes[2],
160			roll = spinBoxes[3], pitch = spinBoxes[4],
161			*yaw = spinBoxes[5];
162
163			osg::Quat quat;
164			getQuatFromEuler(pitch->value(), roll->value(), yaw->value(), quat);
165
166			return "[ " + QString::number(x->value()) + sep +
167			QString::number(y->value()) + sep + QString::number(z->value()) + sep +
168			QString::number(quat.x()) + sep + QString::number(quat.y()) + sep +
169			QString::number(quat.z()) + sep + QString::number(quat.w()) + " ]";
170			}
171
172			#define DEFINE_PROPERTY_EDITOR(Name, Type) \
173			void Name::updateValue() { \
174			Type cfg; \
175			getValues(cfg, spinBoxes); \
176			setPyValue(); \
177			emit valueChanged(cfg); \
178			} \
179			\
180			void Name::setValueFromProperty(viewer::Property* prop) { \
181			Type cfg; \
182			prop->get(cfg); \
183			setValues(cfg, spinBoxes); \
184			setPyValue(); \
185			} \
186			\
187			void Name::set(const Type& v) { \
188			foreach (QDoubleSpinBox* sb, spinBoxes) sb->blockSignals(true); \
189			setValues(v, spinBoxes); \
190			foreach (QDoubleSpinBox* sb, spinBoxes) sb->blockSignals(false); \
191			} \
192			\
193			void Name::setPyValue() { pyValue->setText(valuesToString<Type>(spinBoxes)); }
194
195			DEFINE_PROPERTY_EDITOR(Vector2Dialog, osgVector2)
196			DEFINE_PROPERTY_EDITOR(Vector3Dialog, osgVector3)
197			DEFINE_PROPERTY_EDITOR(Vector4Dialog, osgVector4)
198			DEFINE_PROPERTY_EDITOR(ConfigurationDialog, Configuration)
199
200			template <typename T>
201			QString rowLabel(int i) {
202			return "X" + QString::number(i);
203			}
204
205			template <>
206			QString rowLabel<Configuration>(int i) {
207			switch (i) {
208			case 0:
209			return "X";
210			case 1:
211			return "Y";
212			case 2:
213			return "Z";
214			case 3:
215			return "Roll";
216			case 4:
217			return "Pitch";
218			case 5:
219			return "Yaw";
220			}
221			abort();
222			}
223
224			template <typename Dialog>
225			void constructor_hook(Dialog, QVector<QDoubleSpinBox>) {}
226
227			void constructor_hook(ConfigurationDialog*,
228			QVector<QDoubleSpinBox*> spinBoxes) {
229			foreach (QDoubleSpinBox* sb, spinBoxes) {
230			sb->setSingleStep(0.01);
231			sb->setDecimals(4);
232			}
233			foreach (QDoubleSpinBox* sb, spinBoxes.mid(3))
234			sb->setRange(-osg::PI, osg::PI);
235			}
236
237			#define DEFINE_PROPERTY_EDITOR_CONSTRUCTOR(Name, Type, N) \
238			Name::Name(viewer::Property* property, QWidget* parent) \
239			: QDialog(parent), pyValue(new QLineEdit) { \
240			setModal(false); \
241			pyValue->setReadOnly(true); \
242			\
243			QFormLayout* layout = new QFormLayout; \
244			layout->addRow(new QLabel(property->objectName())); \
245			\
246			spinBoxes.reserve(N); \
247			for (int i = 0; i < N; ++i) { \
248			QDoubleSpinBox* sb(new QDoubleSpinBox); \
249			spinBoxes.append(sb); \
250			setSpinBoxRange(property, sb); \
251			connect(sb, SIGNAL(valueChanged(double)), SLOT(updateValue())); \
252			\
253			layout->addRow(rowLabel<Type>(i), sb); \
254			} \
255			layout->addRow("value", pyValue); \
256			\
257			QDialogButtonBox* buttonBox = \
258			new QDialogButtonBox(QDialogButtonBox::Close, Qt::Horizontal); \
259			connect(buttonBox->button(QDialogButtonBox::Close), SIGNAL(clicked(bool)), \
260			SLOT(accept())); \
261			\
262			layout->addRow(buttonBox); \
263			\
264			setLayout(layout); \
265			\
266			constructor_hook(this, spinBoxes); \
267			}
268
269			DEFINE_PROPERTY_EDITOR_CONSTRUCTOR(Vector2Dialog, osgVector2, 2)
270			DEFINE_PROPERTY_EDITOR_CONSTRUCTOR(Vector3Dialog, osgVector3, 3)
271			DEFINE_PROPERTY_EDITOR_CONSTRUCTOR(Vector4Dialog, osgVector4, 4)
272			DEFINE_PROPERTY_EDITOR_CONSTRUCTOR(ConfigurationDialog, Configuration, 6)
273			} // namespace gui
274			} // namespace gepetto