Initial commit

Lts/Tools/Trajectories/TrajectoryWidget/trajectorywidget.cpp

@@ -0,0 +1,200 @@
+#include "trajectorywidget.h"
+
+#include <QMouseEvent>
+#include <QMatrix4x4>
+#include <QDebug>
+#include <limits>
+#include <algorithm>
+
+TrajectoryWidget::TrajectoryWidget(QWidget *parent)
+    : QOpenGLWidget(parent)
+{
+}
+
+/* ================= OpenGL ================= */
+
+void TrajectoryWidget::initializeGL()
+{
+    initializeOpenGLFunctions();
+    glEnable(GL_DEPTH_TEST);
+
+    const char *vertexShaderSrc = R"(
+        #version 130
+        uniform mat4 mvp;
+        in vec3 position;
+        void main()
+        {
+            gl_Position = mvp * vec4(position, 1.0);
+        }
+    )";
+
+    const char *fragmentShaderSrc = R"(
+        #version 130
+        uniform vec3 color;
+        out vec4 fragColor;
+        void main()
+        {
+            fragColor = vec4(color, 1.0);
+        }
+    )";
+
+    if (!m_program.addShaderFromSourceCode(QOpenGLShader::Vertex, vertexShaderSrc))
+        qDebug() << "Vertex shader error:" << m_program.log();
+
+    if (!m_program.addShaderFromSourceCode(QOpenGLShader::Fragment, fragmentShaderSrc))
+        qDebug() << "Fragment shader error:" << m_program.log();
+
+    if (!m_program.link())
+        qDebug() << "Shader link error:" << m_program.log();
+}
+
+void TrajectoryWidget::resizeGL(int w, int h)
+{
+    glViewport(0, 0, w, h);
+}
+
+void TrajectoryWidget::paintGL()
+{
+    glClearColor(0.15f, 0.15f, 0.2f, 1.0f);
+    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+    if (m_curves.isEmpty())
+        return;
+
+    QMatrix4x4 projection;
+    projection.perspective(
+        45.0f,
+        float(width()) / float(height()),
+        0.01f,
+        100000.0f
+        );
+
+    QMatrix4x4 view;
+    view.translate(0, 0, m_zoom);
+    view.rotate(m_rotX, 1, 0, 0);
+    view.rotate(m_rotY, 0, 1, 0);
+    view.translate(-m_viewCenter);
+
+    QMatrix4x4 mvp = projection * view;
+
+    m_program.bind();
+    m_program.setUniformValue("mvp", mvp);
+
+    for (const auto &curve : m_curves) {
+        if (curve.points.isEmpty())
+            continue;
+
+        m_program.setUniformValue("color", curve.color);
+        m_program.enableAttributeArray(0);
+        m_program.setAttributeArray(
+            0,
+            GL_FLOAT,
+            curve.points.constData(),
+            3
+            );
+
+        glDrawArrays(GL_LINE_STRIP, 0, curve.points.size());
+
+        m_program.disableAttributeArray(0);
+    }
+
+    m_program.release();
+}
+
+/* ================= Public API ================= */
+
+void TrajectoryWidget::setCurves(const QVector<Curve> &curves)
+{
+    m_curves = curves;
+    fitViewToCurves();
+    update();
+}
+
+void TrajectoryWidget::addCurve(const Curve &curve)
+{
+    m_curves.append(curve);
+    fitViewToCurves();
+    update();
+}
+
+void TrajectoryWidget::clearCurves()
+{
+    m_curves.clear();
+    update();
+}
+
+/* ================= View fitting ================= */
+
+void TrajectoryWidget::fitViewToCurves()
+{
+    if (m_curves.isEmpty())
+        return;
+
+    QVector3D minP(
+        std::numeric_limits<float>::max(),
+        std::numeric_limits<float>::max(),
+        std::numeric_limits<float>::max()
+        );
+
+    QVector3D maxP(
+        std::numeric_limits<float>::lowest(),
+        std::numeric_limits<float>::lowest(),
+        std::numeric_limits<float>::lowest()
+        );
+
+    bool hasPoints = false;
+
+    for (const auto &curve : m_curves) {
+        for (const auto &p : curve.points) {
+            minP.setX(std::min(minP.x(), p.x()));
+            minP.setY(std::min(minP.y(), p.y()));
+            minP.setZ(std::min(minP.z(), p.z()));
+
+            maxP.setX(std::max(maxP.x(), p.x()));
+            maxP.setY(std::max(maxP.y(), p.y()));
+            maxP.setZ(std::max(maxP.z(), p.z()));
+
+            hasPoints = true;
+        }
+    }
+
+    if (!hasPoints)
+        return;
+
+    m_viewCenter = (minP + maxP) * 0.5f;
+
+    float dx = maxP.x() - minP.x();
+    float dy = maxP.y() - minP.y();
+    float dz = maxP.z() - minP.z();
+
+    float radius = std::max({dx, dy, dz}) * 0.5f;
+
+    m_zoom = -radius * 2.5f;
+    if (m_zoom > -1.0f)
+        m_zoom = -1.0f;
+}
+
+/* ================= Interaction ================= */
+
+void TrajectoryWidget::mousePressEvent(QMouseEvent *event)
+{
+    m_lastMousePos = event->pos();
+}
+
+void TrajectoryWidget::mouseMoveEvent(QMouseEvent *event)
+{
+    QPoint delta = event->pos() - m_lastMousePos;
+    m_lastMousePos = event->pos();
+
+    if (event->buttons() & Qt::LeftButton) {
+        m_rotX += delta.y();
+        m_rotY += delta.x();
+        update();
+    }
+}
+
+void TrajectoryWidget::wheelEvent(QWheelEvent *event)
+{
+    m_zoom += event->angleDelta().y() * 0.01f;
+    update();
+}