| 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 |
| 275 |
|
|
|