[OrientationVisualizer] Fixed orientation and added axes lines

src/shared/Modules/OrientationVisualizer/OrientationVisualizer.cpp

 #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;
+
+    // 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,8 +137,8 @@ void OrientationVisualizer::setupUi()
     lightEntity->addComponent(lightTransform);
 
     // For camera controls
-    // Qt3DExtras::QFirstPersonCameraController *camController = new
-    // Qt3DExtras::QFirstPersonCameraController(rootEntity);
+    // Qt3DExtras::QFirstPersonCameraController *camController =
+    //     new Qt3DExtras::QFirstPersonCameraController(rootEntity);
     // camController->setLinearSpeed(50.0f);
     // camController->setLookSpeed(180.0f);
     // camController->setCamera(camera);
@@ -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

src/shared/Modules/OrientationVisualizer/OrientationVisualizer.h

 #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
 {
@@ -23,4 +44,9 @@ private:
 
     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};
 };