diff --git a/src/shared/Modules/OrientationVisualizer/OrientationVisualizer.cpp b/src/shared/Modules/OrientationVisualizer/OrientationVisualizer.cpp
index 7a936d5ee0da9e305ada2b3b7209d266446a6588..e435a0a61169edb0a495f877e9f57256318e915f 100644
--- a/src/shared/Modules/OrientationVisualizer/OrientationVisualizer.cpp
+++ b/src/shared/Modules/OrientationVisualizer/OrientationVisualizer.cpp
@@ -1,24 +1,5 @@
#include "OrientationVisualizer.h"
-#include <Qt3DRender/qpointlight.h>
-
-#include <QSizePolicy>
-#include <Qt3DCore/QAspectEngine>
-#include <Qt3DCore/QEntity>
-#include <Qt3DExtras/QConeMesh>
-#include <Qt3DExtras/QFirstPersonCameraController>
-#include <Qt3DExtras/QForwardRenderer>
-#include <Qt3DExtras/QOrbitCameraController>
-#include <Qt3DExtras/QPhongMaterial>
-#include <Qt3DExtras/QSphereMesh>
-#include <Qt3DExtras/QTorusMesh>
-#include <Qt3DExtras/Qt3DWindow>
-#include <Qt3DInput/QInputAspect>
-#include <Qt3DRender/QCamera>
-#include <Qt3DRender/QCameraLens>
-#include <Qt3DRender/QGeometryRenderer>
-#include <Qt3DRender/QRenderAspect>
-
OrientationVisualizer::OrientationVisualizer(QWidget *parent)
: DefaultModule(parent)
{
@@ -29,7 +10,7 @@ OrientationVisualizer::OrientationVisualizer(QWidget *parent)
updateOrientation(0, 0, 0, 1);
getCore()->getMessageBroker()->subscribe(
- Filter::fromString("Mav/ROCKET_FLIGHT_TM"), this,
+ Filter::fromString("Mav/PAYLOAD_FLIGHT_TM"), this,
[this](const Message &message, const Filter &filter)
{
updateOrientation(message.getField("nas_qx").getDouble(),
@@ -49,12 +30,32 @@ XmlObject OrientationVisualizer::toXmlObject()
void OrientationVisualizer::fromXmlObject(const XmlObject &xmlObject) {}
+void OrientationVisualizer::updateOrientation(double qx, double qy, double qz,
+ double qw)
+{
+ float yaw, pitch, roll;
+
+ QQuaternion quaternion(qw, qx, qy, qz);
+ quaternion.getEulerAngles(&pitch, &yaw, &roll);
+
+ yawLabel->setText(QString::asprintf("Y: %.2lf", yaw));
+ pitchLabel->setText(QString::asprintf("P: %.2lf", pitch));
+ rollLabel->setText(QString::asprintf("R: %.2lf", roll));
+
+ QQuaternion displayQuaternion = QQuaternion::fromRotationMatrix(
+ quaternion.toRotationMatrix() * sceneRotation.toRotationMatrix());
+
+ rocketTransform->setRotation(displayQuaternion);
+}
+
void OrientationVisualizer::setupUi()
{
Qt3DExtras::Qt3DWindow *view = new Qt3DExtras::Qt3DWindow;
QWidget *windowContainer = QWidget::createWindowContainer(view);
- Qt3DCore::QEntity *rootEntity = new Qt3DCore::QEntity;
+ Qt3DCore::QEntity *rootEntity = new Qt3DCore::QEntity;
+
+ // Cone
Qt3DCore::QEntity *rocketEntity = new Qt3DCore::QEntity(rootEntity);
Qt3DExtras::QConeMesh *rocketMesh = new Qt3DExtras::QConeMesh();
@@ -74,11 +75,17 @@ void OrientationVisualizer::setupUi()
rocketEntity->addComponent(rocketMaterial);
rocketEntity->addComponent(rocketTransform);
+ // Axes lines
+ drawLine({0, 0, 0}, {100, 0, 0}, QColorConstants::Red,
+ rootEntity); // X
+ drawLine({0, 0, 0}, {0, 100, 0}, QColorConstants::Green, rootEntity); // Y
+ drawLine({0, 0, 0}, {0, 0, 100}, QColorConstants::Blue, rootEntity); // Z
+
// Spheres
- Qt3DExtras::QSphereMesh *sphereMesh = new Qt3DExtras::QSphereMesh;
- sphereMesh->setRadius(3);
- sphereMesh->setGenerateTangents(true);
+ // Qt3DExtras::QSphereMesh *sphereMesh = new Qt3DExtras::QSphereMesh;
+ // sphereMesh->setRadius(3);
+ // sphereMesh->setGenerateTangents(true);
// Qt3DCore::QEntity *sphereEntity1 = new Qt3DCore::QEntity(rootEntity);
// Qt3DCore::QTransform *sphereTransform1 = new Qt3DCore::QTransform;
@@ -114,9 +121,9 @@ void OrientationVisualizer::setupUi()
Qt3DRender::QCamera *camera = view->camera();
camera->lens()->setPerspectiveProjection(45.0f, 16.0f / 9.0f, 0.1f,
1000.0f);
- camera->setPosition(QVector3D(30.0f, 30.0f, 30.0f));
+ camera->setPosition(QVector3D(-30, -30, -30));
camera->setViewCenter(QVector3D(0, 0, 0));
- camera->setUpVector(QVector3D(-1, -1, 1));
+ camera->setUpVector(QVector3D(0, 0, -60));
// Light
Qt3DCore::QEntity *lightEntity = new Qt3DCore::QEntity(rootEntity);
@@ -130,10 +137,10 @@ void OrientationVisualizer::setupUi()
lightEntity->addComponent(lightTransform);
// For camera controls
- // Qt3DExtras::QFirstPersonCameraController *camController = new
- // Qt3DExtras::QFirstPersonCameraController(rootEntity);
- // camController->setLinearSpeed( 50.0f );
- // camController->setLookSpeed( 180.0f );
+ // Qt3DExtras::QFirstPersonCameraController *camController =
+ // new Qt3DExtras::QFirstPersonCameraController(rootEntity);
+ // camController->setLinearSpeed(50.0f);
+ // camController->setLookSpeed(180.0f);
// camController->setCamera(camera);
view->setRootEntity(rootEntity);
@@ -164,21 +171,72 @@ void OrientationVisualizer::setupUi()
container->setStretch(1, 0);
}
-void OrientationVisualizer::updateOrientation(double qx, double qy, double qz,
- double qw)
+void OrientationVisualizer::drawLine(const QVector3D &start,
+ const QVector3D &end, const QColor &color,
+ Qt3DCore::QEntity *rootEntity)
{
- float yaw, pitch, roll;
-
- QQuaternion quaternion(QVector4D(qx, qy, qz, qw));
- quaternion.getEulerAngles(&pitch, &yaw, &roll);
-
- yawLabel->setText(QString::asprintf("Y: %.2lf", yaw));
- pitchLabel->setText(QString::asprintf("P: %.2lf", pitch));
- rollLabel->setText(QString::asprintf("R: %.2lf", roll));
-
- QQuaternion displayQuaternion = QQuaternion::fromRotationMatrix(
- quaternion.toRotationMatrix() *
- QQuaternion::fromEulerAngles(90, 0, 0).toRotationMatrix());
-
- rocketTransform->setRotation(displayQuaternion);
-}
+ auto *geometry = new Qt3DRender::QGeometry(rootEntity);
+
+ // Position vertices (start and end)
+ QByteArray bufferBytes;
+ bufferBytes.resize(
+ 3 * 2 *
+ sizeof(float)); // start.x, start.y, start.end + end.x, end.y, end.z
+ float *positions = reinterpret_cast<float *>(bufferBytes.data());
+ *positions++ = start.x();
+ *positions++ = start.y();
+ *positions++ = start.z();
+ *positions++ = end.x();
+ *positions++ = end.y();
+ *positions++ = end.z();
+
+ auto *buf = new Qt3DRender::QBuffer(geometry);
+ buf->setData(bufferBytes);
+
+ auto *positionAttribute = new Qt3DRender::QAttribute(geometry);
+ positionAttribute->setName(
+ Qt3DRender::QAttribute::defaultPositionAttributeName());
+ positionAttribute->setVertexBaseType(Qt3DRender::QAttribute::Float);
+ positionAttribute->setVertexSize(3);
+ positionAttribute->setAttributeType(
+ Qt3DRender::QAttribute::VertexAttribute);
+ positionAttribute->setBuffer(buf);
+ positionAttribute->setByteStride(3 * sizeof(float));
+ positionAttribute->setCount(2);
+ geometry->addAttribute(
+ positionAttribute); // We add the vertices in the geometry
+
+ // Connectivity between vertices
+ QByteArray indexBytes;
+ indexBytes.resize(2 * sizeof(unsigned int)); // start to end
+ unsigned int *indices = reinterpret_cast<unsigned int *>(indexBytes.data());
+ *indices++ = 0;
+ *indices++ = 1;
+
+ auto *indexBuffer = new Qt3DRender::QBuffer(geometry);
+ indexBuffer->setData(indexBytes);
+
+ auto *indexAttribute = new Qt3DRender::QAttribute(geometry);
+ indexAttribute->setVertexBaseType(Qt3DRender::QAttribute::UnsignedInt);
+ indexAttribute->setAttributeType(Qt3DRender::QAttribute::IndexAttribute);
+ indexAttribute->setBuffer(indexBuffer);
+ indexAttribute->setCount(2);
+ geometry->addAttribute(indexAttribute); // We add the indices linking the
+ // points in the geometry
+
+ // Mesh
+ auto *line = new Qt3DRender::QGeometryRenderer(rootEntity);
+ line->setGeometry(geometry);
+ line->setPrimitiveType(Qt3DRender::QGeometryRenderer::Lines);
+ auto *material = new Qt3DExtras::QPhongMaterial(rootEntity);
+ material->setAmbient(color);
+
+ // Entity
+ auto *lineEntity = new Qt3DCore::QEntity(rootEntity);
+ lineEntity->addComponent(line);
+ lineEntity->addComponent(material);
+
+ // auto *transform = new Qt3DCore::QTransform();
+ // transform->setRotation(sceneRotation);
+ // lineEntity->addComponent(transform);
+}
\ No newline at end of file
diff --git a/src/shared/Modules/OrientationVisualizer/OrientationVisualizer.h b/src/shared/Modules/OrientationVisualizer/OrientationVisualizer.h
index b7192d3ece2434c4814083424d9cc08c8eeb4c6f..eb9030050753b73ea27525fd2923ed56cfdd9a94 100644
--- a/src/shared/Modules/OrientationVisualizer/OrientationVisualizer.h
+++ b/src/shared/Modules/OrientationVisualizer/OrientationVisualizer.h
@@ -1,26 +1,52 @@
#pragma once
#include <Modules/DefaultModule/DefaultModule.h>
+#include <Qt3DRender/qpointlight.h>
+#include <QSizePolicy>
+#include <Qt3DCore/QAspectEngine>
+#include <Qt3DCore/QEntity>
#include <Qt3DCore/QTransform>
+#include <Qt3DExtras/QConeMesh>
+#include <Qt3DExtras/QCuboidMesh>
+#include <Qt3DExtras/QFirstPersonCameraController>
+#include <Qt3DExtras/QForwardRenderer>
+#include <Qt3DExtras/QOrbitCameraController>
+#include <Qt3DExtras/QPhongMaterial>
+#include <Qt3DExtras/QSphereMesh>
+#include <Qt3DExtras/QTorusMesh>
+#include <Qt3DExtras/Qt3DWindow>
+#include <Qt3DInput/QInputAspect>
+#include <Qt3DRender/QAttribute>
+#include <Qt3DRender/QBuffer>
+#include <Qt3DRender/QCamera>
+#include <Qt3DRender/QCameraLens>
+#include <Qt3DRender/QGeometry>
+#include <Qt3DRender/QGeometryRenderer>
+#include <Qt3DRender/QRenderAspect>
class OrientationVisualizer : public DefaultModule
{
Q_OBJECT
public:
- explicit OrientationVisualizer(QWidget* parent = nullptr);
+ explicit OrientationVisualizer(QWidget *parent = nullptr);
- QWidget* toWidget() override;
+ QWidget *toWidget() override;
XmlObject toXmlObject() override;
- void fromXmlObject(const XmlObject& xmlObject) override;
+ void fromXmlObject(const XmlObject &xmlObject) override;
private:
QLabel *rollLabel, *pitchLabel, *yawLabel;
- Qt3DCore::QTransform* rocketTransform;
+ Qt3DCore::QTransform *rocketTransform;
void updateOrientation(double qx, double qy, double qz, double qw);
void setupUi();
+
+ void drawLine(const QVector3D &start, const QVector3D &end,
+ const QColor &color, Qt3DCore::QEntity *rootEntity);
+
+ QQuaternion sceneRotation = {-0.5, -0.5, -0.5, 0.5};
};