| Line |
Branch |
Exec |
Source |
| 1 |
|
|
// |
| 2 |
|
|
// Copyright (c) CNRS |
| 3 |
|
|
// Authors: Joseph Mirabel and Heidy Dallard |
| 4 |
|
|
// |
| 5 |
|
|
|
| 6 |
|
|
#include "transformwidget.hh" |
| 7 |
|
|
|
| 8 |
|
|
#include <QVBoxLayout> |
| 9 |
|
|
|
| 10 |
|
|
#include "gepetto/gui/mainwindow.hh" |
| 11 |
|
|
|
| 12 |
|
|
namespace hpp { |
| 13 |
|
|
namespace gui { |
| 14 |
|
|
namespace { |
| 15 |
|
✗ |
QDoubleSpinBox* makeSpinBox(double val, TransformWidget* recv, const char* slot, |
| 16 |
|
|
double bound) { |
| 17 |
|
✗ |
QDoubleSpinBox* sb = new QDoubleSpinBox; |
| 18 |
|
✗ |
sb->setDecimals(10 /*DBL_MAX_10_EXP + DBL_DIG*/); |
| 19 |
|
✗ |
sb->setMinimum(-bound); |
| 20 |
|
✗ |
sb->setMaximum(bound); |
| 21 |
|
✗ |
sb->setSingleStep(0.01); |
| 22 |
|
✗ |
sb->setValue(val); |
| 23 |
|
✗ |
recv->connect(sb, SIGNAL(valueChanged(double)), slot); |
| 24 |
|
✗ |
return sb; |
| 25 |
|
|
} |
| 26 |
|
|
} // namespace |
| 27 |
|
|
|
| 28 |
|
✗ |
TransformWidget::TransformWidget(hpp::Transform__slice* transform, |
| 29 |
|
|
std::string const& jointName, QWidget* parent, |
| 30 |
|
✗ |
bool doPosition, bool doQuaternion) |
| 31 |
|
✗ |
: QDialog(parent, Qt::Dialog | Qt::WindowStaysOnTopHint), transform_(NULL) { |
| 32 |
|
✗ |
transform_ = transform; |
| 33 |
|
✗ |
QQuaternion q((float)transform_[6], (float)transform_[3], |
| 34 |
|
✗ |
(float)transform_[4], (float)transform_[5]); |
| 35 |
|
✗ |
q.normalize(); |
| 36 |
|
✗ |
qDebug() << q; |
| 37 |
|
|
rAxis_ = |
| 38 |
|
✗ |
q.vector().normalized() * 2 * std::atan2(q.vector().length(), q.scalar()); |
| 39 |
|
✗ |
jointName_ = jointName; |
| 40 |
|
✗ |
QVBoxLayout* l = new QVBoxLayout; |
| 41 |
|
✗ |
this->setLayout(l); |
| 42 |
|
|
|
| 43 |
|
✗ |
if (doPosition) { |
| 44 |
|
✗ |
l->addWidget(new QLabel("Position", this)); |
| 45 |
|
✗ |
xSlider_ = |
| 46 |
|
✗ |
makeSpinBox(transform_[0], this, SLOT(xChanged(double)), DBL_MAX); |
| 47 |
|
✗ |
l->addWidget(xSlider_); |
| 48 |
|
|
|
| 49 |
|
✗ |
ySlider_ = |
| 50 |
|
✗ |
makeSpinBox(transform_[1], this, SLOT(yChanged(double)), DBL_MAX); |
| 51 |
|
✗ |
l->addWidget(ySlider_); |
| 52 |
|
|
|
| 53 |
|
✗ |
zSlider_ = |
| 54 |
|
✗ |
makeSpinBox(transform_[2], this, SLOT(zChanged(double)), DBL_MAX); |
| 55 |
|
✗ |
l->addWidget(zSlider_); |
| 56 |
|
|
} |
| 57 |
|
|
|
| 58 |
|
✗ |
if (doQuaternion) { |
| 59 |
|
✗ |
l->addWidget(new QLabel("Quaternion", this)); |
| 60 |
|
✗ |
xQuaternion_ = |
| 61 |
|
✗ |
makeSpinBox(rAxis_.x(), this, SLOT(xRotateChanged(double)), M_PI); |
| 62 |
|
✗ |
l->addWidget(xQuaternion_); |
| 63 |
|
|
|
| 64 |
|
✗ |
yQuaternion_ = |
| 65 |
|
✗ |
makeSpinBox(rAxis_.y(), this, SLOT(yRotateChanged(double)), M_PI); |
| 66 |
|
✗ |
l->addWidget(yQuaternion_); |
| 67 |
|
|
|
| 68 |
|
✗ |
zQuaternion_ = |
| 69 |
|
✗ |
makeSpinBox(rAxis_.z(), this, SLOT(zRotateChanged(double)), M_PI); |
| 70 |
|
✗ |
l->addWidget(zQuaternion_); |
| 71 |
|
|
} |
| 72 |
|
|
|
| 73 |
|
✗ |
setWindowTitle(QString::fromStdString("Transform ")); |
| 74 |
|
✗ |
setAttribute(Qt::WA_DeleteOnClose); |
| 75 |
|
|
} |
| 76 |
|
|
|
| 77 |
|
✗ |
void TransformWidget::changed(bool axisChanged) { |
| 78 |
|
✗ |
if (axisChanged) { |
| 79 |
|
✗ |
QQuaternion quaternion; |
| 80 |
|
✗ |
if (!rAxis_.isNull()) { |
| 81 |
|
✗ |
const float theta = rAxis_.length(); |
| 82 |
|
✗ |
quaternion = QQuaternion(std::cos(theta / 2), |
| 83 |
|
✗ |
std::sin(theta / 2) * rAxis_ / theta); |
| 84 |
|
|
} |
| 85 |
|
|
|
| 86 |
|
✗ |
transform_[6] = quaternion.scalar(); |
| 87 |
|
✗ |
transform_[3] = quaternion.x(); |
| 88 |
|
✗ |
transform_[4] = quaternion.y(); |
| 89 |
|
✗ |
transform_[5] = quaternion.z(); |
| 90 |
|
|
} |
| 91 |
|
✗ |
emit valueChanged(transform_, jointName_); |
| 92 |
|
|
} |
| 93 |
|
|
|
| 94 |
|
✗ |
void TransformWidget::xChanged(double value) { |
| 95 |
|
✗ |
transform_[0] = value; |
| 96 |
|
✗ |
changed(); |
| 97 |
|
|
} |
| 98 |
|
|
|
| 99 |
|
✗ |
void TransformWidget::yChanged(double value) { |
| 100 |
|
✗ |
transform_[1] = value; |
| 101 |
|
✗ |
changed(); |
| 102 |
|
|
} |
| 103 |
|
|
|
| 104 |
|
✗ |
void TransformWidget::zChanged(double value) { |
| 105 |
|
✗ |
transform_[2] = value; |
| 106 |
|
✗ |
changed(); |
| 107 |
|
|
} |
| 108 |
|
|
|
| 109 |
|
✗ |
void TransformWidget::xRotateChanged(double value) { |
| 110 |
|
✗ |
rAxis_.setX((float)value); |
| 111 |
|
✗ |
changed(true); |
| 112 |
|
|
} |
| 113 |
|
|
|
| 114 |
|
✗ |
void TransformWidget::yRotateChanged(double value) { |
| 115 |
|
✗ |
rAxis_.setY((float)value); |
| 116 |
|
✗ |
changed(true); |
| 117 |
|
|
} |
| 118 |
|
|
|
| 119 |
|
✗ |
void TransformWidget::zRotateChanged(double value) { |
| 120 |
|
✗ |
rAxis_.setZ((float)value); |
| 121 |
|
✗ |
changed(true); |
| 122 |
|
|
} |
| 123 |
|
|
} // namespace gui |
| 124 |
|
|
} // namespace hpp |
| 125 |
|
|
|