Proof-of-concept KSP-Style NavBall visualization for orientation [WIP]

manifest.xml

@@ -24,6 +24,9 @@
   <qtwidget name="OrientationView">
     <image>resources/orientationView1.jpeg</image>
   </qtwidget>
+  <qtwidget name="NavBallView">
+    <image>resources/orientationView1.jpeg</image>
+  </qtwidget>
   <qtwidget name="RangeView">
     <image>resources/rangeView1.jpeg</image>
   </qtwidget>

src/CMakeLists.txt

@@ -84,6 +84,8 @@ SET(COLLECTION_MOC_HDRS
   sonar_widget/SonarPlot.h
   sonar_widget/SonarWidget.h
   sonar_widget/SonarWidgetPlugin.h
+  nav_ball/navball.h
+  nav_ball/navballplugin.h
 )
 
 SET(COLLECTION_HDRS
@@ -127,6 +129,8 @@ SET(COLLECTION_HDRS
   sonar_widget/SonarPlot.h
   sonar_widget/SonarWidget.h
   sonar_widget/SonarWidgetPlugin.h
+  nav_ball/navball.h
+  nav_ball/navballplugin.h
 )
 
 FILE(GLOB COLLECTION_SOURCES 
@@ -143,6 +147,7 @@ FILE(GLOB COLLECTION_SOURCES
     image_view/*.cc
     progress_indicator/*.cc
     sonar_widget/*.cc
+    nav_ball/*.cc
     *.cc)
 
 if (USE_VTK)
@@ -176,6 +181,7 @@ rock_vizkit_widget(rock_widget_collection SHARED
     HEADERS ${COLLECTION_HDRS}
     MOC ${COLLECTION_MOC_HDRS}
     DEPS_PKGCONFIG base-types base-lib frame_helper
+                   QtOpenGL
     DEPS_CMAKE QWT OpenGL ${VTK_DEPS}
     LIBS ${QT_QTCORE_LIBRARY} ${VTK_LIBS} ${QT_QTGUI_LIBRARY} ${QT_QTOPENGL_LIBRARY} ${QT_QTDESIGNER_LIBRARY} ${QTGSTREAMER_LIBRARIES} ${QTGSTREAMER_UI_LIBRARIES} 
     )

src/RockWidgetCollection.cc

@@ -17,6 +17,8 @@
 #include "progress_indicator/ProgressIndicatorPlugin.h"
 #include "sonar_widget/SonarWidgetPlugin.h"
 
+#include "nav_ball/navballplugin.h"
+
 #ifdef USE_VTK
 #include "vtk/sonar_display/SonarDisplayPlugin.h"
 #include "vtk/vectorfield3D/vectorfield3DPlugin.h"
@@ -35,6 +37,7 @@ RockWidgetCollection::RockWidgetCollection(QObject *parent)
    widgets.append(new MultiViewPlugin(this));
    widgets.append(new MultiWidgetPlugin(this));
    widgets.append(new OrientationPlugin(this));
+   widgets.append(new NavBallPlugin(this));
    widgets.append(new Plot2dPlugin(this));
    widgets.append(new ProgressIndicatorPlugin(this));
    widgets.append(new RangeViewPlugin(this));

src/nav_ball/navball.cc

@@ -0,0 +1,124 @@
+#include "navball.h"
+#include <QHBoxLayout>
+#include <iostream>
+
+#include<GL/glut.h>
+
+
+NavBallView::NavBallView(QWidget* parent) : QGLWidget(parent), texture(0)
+{
+    setMinimumWidth(100);
+    setMinimumHeight(100);
+
+    Eigen::Matrix3f NWU;
+    NWU <<
+            +0,+1,+0,
+            +0,+0,+1,
+            +1,+0,+0;
+    nwu2gl = NWU.transpose();
+//    std::cout << "NWU2GL: " << nwu2gl.coeffs().transpose() << '\n';
+}
+
+void NavBallView::initializeGL()
+{
+    makeSphere();
+    glEnable(GL_DEPTH_TEST);
+    glEnable(GL_CULL_FACE);
+    glEnable(GL_TEXTURE_2D);
+
+    // optional: (but light position should be fixed relative to camera not to object)
+//    glShadeModel(GL_SMOOTH);
+//    glEnable(GL_LIGHTING);
+//    glEnable(GL_LIGHT0);
+//    glEnable(GL_MULTISAMPLE);
+//    static GLfloat lightPosition[4] = { 0.5, 5.0, 7.0, 1.0 };
+//    glLightfv(GL_LIGHT0, GL_POSITION, lightPosition);
+}
+
+void NavBallView::resizeGL(int width, int height)
+{
+    int side = std::min(width, height);
+    glViewport((width - side) / 2, (height - side) / 2, side, side);
+
+    glMatrixMode(GL_PROJECTION);
+    glLoadIdentity();
+    glOrtho(-1, +1, -1, +1, 4.0, 4.9);
+    glMatrixMode(GL_MODELVIEW);
+}
+
+void NavBallView::paintGL()
+{
+    qglClearColor(Qt::black);
+    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+    Eigen::Quaternionf res = quat * nwu2gl;
+    res.x()*=-1.f; // invert tilt rotation
+    Eigen::Transform<float, 3, Eigen::Affine, Eigen::ColMajor> trafo(res);
+    trafo.translation() = Eigen::Vector3f::UnitZ() * -5.f;
+    glLoadMatrixf(trafo.data());
+
+    glEnableClientState(GL_VERTEX_ARRAY);
+    glEnableClientState(GL_INDEX_ARRAY);
+    glEnableClientState(GL_TEXTURE_COORD_ARRAY);
+    glIndexPointer(GL_UNSIGNED_INT, 0, indices.data());
+    glVertexPointer(3, GL_FLOAT, 0, vertices.constData());
+    glTexCoordPointer(2, GL_FLOAT, 0, texCoords.constData());
+    glBindTexture(GL_TEXTURE_2D, texture);
+
+    glDrawElements(GL_TRIANGLES, indices.size(), GL_UNSIGNED_INT, indices.data());
+}
+
+void NavBallView::setQuaternion(const QQuaternion& q)
+{
+//    std::cout << "setQuaternion([" << q.x() << ' ' << q.y() << ' ' << q.z() << ", " << q.scalar() << "]\n";
+    quat.w() = q.scalar();
+    quat.x() = q.x();
+    quat.y() = q.y();
+    quat.z() = q.z();
+    updateGL();
+}
+
+
+void NavBallView::makeSphere()
+{
+    QPixmap pixmap(QString(":/nav_ball/navball.png"));
+//    std::cout << "Image: " << pixmap.size().height() << " x " << pixmap.size().width() << '\n';
+    texture = bindTexture(pixmap, GL_TEXTURE_2D);
+    const int meridians = 32, latitudes = 15;
+    vertices.reserve((meridians+1) * (latitudes+2));
+    texCoords.reserve((meridians+1) * (latitudes+2));
+
+    for (int i = 0; i < meridians + 1; ++i) {
+        for (int j = 0; j < latitudes + 2; ++j) {
+            // texture coordinates in [0,1] x [0,1]:
+            texCoords.append(QVector2D(double(i)/meridians, double(j)/(latitudes+1)));
+
+            // theta = longitude from 0 to 2*pi
+            // phi = latitude from -pi/2 to pi/2
+            double theta = 2*M_PI * (texCoords.back().x());
+            double phi = M_PI * (texCoords.back().y() - 0.5);
+            vertices.append( QVector3D(
+                    std::cos(phi) * std::cos(theta),
+                    std::cos(phi) * std::sin(theta),
+                    std::sin(phi)
+            ));
+        }
+    }
+
+    for (int i = 0; i < meridians; ++i)
+    {
+        // Construct triangles between successive meridians
+        for (int j = 0; j < latitudes + 1; ++j)
+        {
+            indices.push_back((i+1) * (latitudes+2) + j+1);
+            indices.push_back(i * (latitudes+2) + j+1);
+            indices.push_back(i * (latitudes+2) + j);
+
+            indices.push_back(i * (latitudes+2) + j);
+            indices.push_back((i+1) * (latitudes+2) + j);
+            indices.push_back((i+1) * (latitudes+2) + j+1);
+        }
+    }
+//    std::cout << "Made sphere, vertices: " << vertices.size() << ", indices: " << indices.size() << '\n';
+}
+

src/nav_ball/navball.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#include <QtGui>
+#include <QGLWidget>
+
+#include <QQuaternion>
+
+#include <Eigen/Geometry>
+
+class QWidget;
+
+class NavBallView : public QGLWidget
+{
+    Q_OBJECT
+    Q_CLASSINFO("Author", "Christoph Hertzberg")
+
+public:
+    NavBallView(QWidget *parent=NULL);
+    //    virtual ~Orientation(); //Attention if you comment in this the widgets are not longer visible in designer WTF?
+    void paintEvent(QPaintEvent *) {paintGL();}
+    void resizeEvent ( QResizeEvent * event ) {QGLWidget::resizeEvent(event);}
+
+public slots:
+
+    NavBallView* newInstance() { return new NavBallView; } // TODO why?
+    void setQuaternion(const QQuaternion& q);
+    void update() const {}; //Nothing needs to be done but needs to be implemented for ruby bindings
+
+protected:
+    void initializeGL();
+    void paintGL();
+    void resizeGL(int width, int height);
+private:
+    void makeSphere();
+
+    GLuint texture;
+    QVector<QVector3D> vertices;
+    QVector<QVector2D> texCoords;
+    std::vector<unsigned int> indices;
+
+    Eigen::Quaternion<float, Eigen::Unaligned> quat, nwu2gl;
+};
+

src/nav_ball/navballplugin.cc

@@ -0,0 +1,84 @@
+#include "navballplugin.h"
+#include "navball.h"
+
+ NavBallPlugin::NavBallPlugin(QObject *parent) : QObject(parent)
+ {
+     initialized = false;
+ }
+
+ NavBallPlugin::~NavBallPlugin()
+ {
+ }
+
+ void NavBallPlugin::initialize(QDesignerFormEditorInterface *formEditor)
+ {
+     if (initialized)
+         return;
+     initialized = true;
+ }
+
+ bool NavBallPlugin::isInitialized() const
+ {
+     return initialized;
+ }
+
+ QWidget *NavBallPlugin::createWidget(QWidget *parent)
+ {
+     return new NavBallView(parent);
+ }
+
+ QString NavBallPlugin::name() const
+ {
+     return "NavBallView";
+ }
+
+ QString NavBallPlugin::group() const
+ {
+     return "Rock-Robotics";
+ }
+
+ QIcon NavBallPlugin::icon() const
+ {
+     return QIcon(":/artificial_horizon/icon.png");
+ }
+
+ QString NavBallPlugin::toolTip() const
+ {
+     return "Widget for displaying an Orientation";
+ }
+
+ QString NavBallPlugin::whatsThis() const
+ {
+     return "Widget for displaying an Orientation";
+ }
+
+ bool NavBallPlugin::isContainer() const
+ {
+     return false;
+ }
+
+ QString NavBallPlugin::domXml() const
+ {
+     return "<widget class=\"NavBallView\" name=\"NavBall\">\n"
+            " <property name=\"geometry\">\n"
+            "  <rect>\n"
+            "   <x>0</x>\n"
+            "   <y>0</y>\n"
+            "   <width>150</width>\n"
+            "   <height>150</height>\n"
+            "  </rect>\n"
+            " </property>\n"
+            " <property name=\"toolTip\" >\n"
+            "  <string>NavBallView</string>\n"
+            " </property>\n"
+            " <property name=\"whatsThis\" >\n"
+            "  <string></string>\n"
+            " </property>\n"
+            "</widget>\n";
+ }
+
+ QString NavBallPlugin::includeFile() const
+ {
+     return "rock_widget_collection/navball.h";
+ }
+

src/nav_ball/navballplugin.h

@@ -0,0 +1,30 @@
+
+#pragma once
+
+#include <QtGui>
+#include <QtDesigner/QDesignerCustomWidgetInterface>
+
+class NavBallPlugin : public QObject , public QDesignerCustomWidgetInterface
+{
+  Q_OBJECT
+  Q_INTERFACES(QDesignerCustomWidgetInterface)
+
+ public:
+   NavBallPlugin(QObject *parent = 0);
+   virtual ~NavBallPlugin();
+
+   bool isContainer() const;
+   bool isInitialized() const;
+   QIcon icon() const;
+   QString domXml() const;
+   QString group() const;
+   QString includeFile() const;
+   QString name() const;
+   QString toolTip() const;
+   QString whatsThis() const;
+   QWidget *createWidget(QWidget *parent);
+   void initialize(QDesignerFormEditorInterface *core);
+
+ private:
+   bool initialized; 
+};

src/resources.qrc

@@ -13,5 +13,6 @@
     <file alias="timeline/icon">timeline/resources/icon.png</file>
     <file alias="progress_indicator/icon">progress_indicator/resources/icon.png</file>
     <file alias="virtual_joystick/icon.png">virtual_joystick/resources/icon.png</file>
+    <file alias="nav_ball/navball.png">nav_ball/resources/navball.png</file>
  </qresource>
  </RCC>