Added 3D visualization from Lionel

qgroundcontrol.pro

@@ -89,7 +89,8 @@ INCLUDEPATH += src \
     src/lib/qmapcontrol \
     src/ui/mavlink \
     src/ui/param \
-    src/ui/watchdog
+    src/ui/watchdog \
+    src/ui/map3D
 HEADERS += src/MG.h \
     src/Core.h \
     src/uas/UASInterface.h \
@@ -160,7 +161,11 @@ HEADERS += src/MG.h \
     src/ui/map/QGC2DIcon.h \
     src/ui/QGCRemoteControlView.h \
     src/WaypointGlobal.h \
-    src/ui/WaypointGlobalView.h
+    src/ui/WaypointGlobalView.h \
+    src/ui/map3D/Q3DWidget.h \
+    src/ui/map3D/CheetahModel.h \
+    src/ui/map3D/CheetahGL.h \
+    src/ui/map3D/QMap3DWidget.h
 SOURCES += src/main.cc \
     src/Core.cc \
     src/uas/UASManager.cc \
@@ -226,7 +231,11 @@ SOURCES += src/main.cc \
     src/ui/map/QGC2DIcon.cc \
     src/ui/QGCRemoteControlView.cc \
     src/WaypointGlobal.cpp \
-    src/ui/WaypointGlobalView.cpp
+    src/ui/WaypointGlobalView.cpp \
+    src/ui/map3D/Q3DWidget.cc \
+    src/ui/map3D/CheetahModel.cc \
+    src/ui/map3D/CheetahGL.cc \
+    src/ui/map3D/QMap3DWidget.cc
 RESOURCES = mavground.qrc
 
 # Include RT-LAB Library

src/uas/UAS.cc

@@ -73,6 +73,12 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
         sendDropRate(0),
         lowBattAlarm(false),
         positionLock(false),
+        localX(0),
+        localY(0),
+        localZ(0),
+        roll(0),
+        pitch(0),
+        yaw(0),
         statusTimeout(new QTimer(this))
 {
     color = UASInterface::getNextColor();
@@ -281,6 +287,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 mavlink_attitude_t attitude;
                 mavlink_msg_attitude_decode(&message, &attitude);
                 quint64 time = getUnixTime(attitude.usec);
+                roll = attitude.roll;
+                pitch = attitude.pitch;
+                yaw = attitude.yaw;
                 emit valueChanged(uasId, "roll IMU", mavlink_msg_attitude_get_roll(&message), time);
                 emit valueChanged(uasId, "pitch IMU", mavlink_msg_attitude_get_pitch(&message), time);
                 emit valueChanged(uasId, "yaw IMU", mavlink_msg_attitude_get_yaw(&message), time);
@@ -300,6 +309,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 mavlink_local_position_t pos;
                 mavlink_msg_local_position_decode(&message, &pos);
                 quint64 time = getUnixTime(pos.usec);
+                localX = pos.x;
+                localY = pos.y;
+                localZ = pos.z;
                 emit valueChanged(uasId, "x", pos.x, time);
                 emit valueChanged(uasId, "y", pos.y, time);
                 emit valueChanged(uasId, "z", pos.z, time);

src/uas/UAS.h

@@ -79,6 +79,15 @@ public:
     /** @brief Get the links associated with this robot */
     QList<LinkInterface*>* getLinks();
 
+    double getLocalX() const { return localX; };
+    double getLocalY() const { return localY; };
+    double getLocalZ() const { return localZ; };
+
+    double getRoll() const { return roll; };
+    double getPitch() const { return pitch; };
+    double getYaw() const { return yaw; };
+
+
 friend class UASWaypointManager;
 protected:
     int uasId;                    ///< Unique system ID
@@ -127,6 +136,12 @@ protected:
     float sendDropRate;         ///< Percentage of packets that were not received from the MAV by the GCS
     bool lowBattAlarm;          ///< Switch if battery is low
     bool positionLock;          ///< Status if position information is available or not
+    double localX;
+    double localY;
+    double localZ;
+    double roll;
+    double pitch;
+    double yaw;
     QTimer* statusTimeout;      ///< Timer for various status timeouts
 
     /** @brief Set the current battery type */

src/uas/UASInterface.h

@@ -64,6 +64,14 @@ public:
     /** @brief Get the status flag for the communication **/
     virtual int getCommunicationStatus() const = 0;
 
+    virtual double getLocalX() const = 0;
+    virtual double getLocalY() const = 0;
+    virtual double getLocalZ() const = 0;
+
+    virtual double getRoll() const = 0;
+    virtual double getPitch() const = 0;
+    virtual double getYaw() const = 0;
+
     /** @brief Get reference to the waypoint manager **/
     virtual UASWaypointManager &getWaypointManager(void) = 0;
 

src/ui/MainWindow.cc

@@ -45,6 +45,7 @@ This file is part of the QGROUNDCONTROL project
 #include "MainWindow.h"
 #include "JoystickWidget.h"
 #include "GAudioOutput.h"
+#include "QMap3DWidget.h"
 
 // FIXME Move
 #include "PxQuadMAV.h"
@@ -131,7 +132,7 @@ void MainWindow::buildWidgets()
     mapWidget         = new MapWidget(this);
     protocolWidget    = new XMLCommProtocolWidget(this);
     dataplotWidget    = new QGCDataPlot2D(this);
-    map3DWidget       = new QWidget(this); // FIXME Lionel, insert visualizer here
+    map3DWidget       = new QMap3DWidget(this);
 
     // Dock widgets
     controlDockWidget = new QDockWidget(tr("Control"), this);
@@ -216,6 +217,7 @@ void MainWindow::arrangeCenterStack()
     if (linechartWidget) centerStack->addWidget(linechartWidget);
     if (protocolWidget) centerStack->addWidget(protocolWidget);
     if (mapWidget) centerStack->addWidget(mapWidget);
+    if (map3DWidget) centerStack->addWidget(map3DWidget);
     if (hudWidget) centerStack->addWidget(hudWidget);
     if (dataplotWidget) centerStack->addWidget(dataplotWidget);
 

src/ui/MainWindow.h

@@ -63,6 +63,7 @@ This file is part of the QGROUNDCONTROL project
 #include "HSIDisplay.h"
 #include "QGCDataPlot2D.h"
 #include "QGCRemoteControlView.h"
+#include "QMap3DWidget.h"
 
 #include "LogCompressor.h"
 
@@ -160,7 +161,7 @@ protected:
     QPointer<MapWidget> mapWidget;
     QPointer<XMLCommProtocolWidget> protocolWidget;
     QPointer<QGCDataPlot2D> dataplotWidget;
-    QPointer<QWidget> map3DWidget;
+    QPointer<QMap3DWidget> map3DWidget;
     // Dock widgets
     QPointer<QDockWidget> controlDockWidget;
     QPointer<QDockWidget> infoDockWidget;

src/ui/map3D/CheetahGL.h

+#ifndef CHEETAHGL_H_
+#define CHEETAHGL_H_
+
+#include <GL/gl.h>
+
+GLint generatePixhawkCheetah(float red, float green, float blue);
+
+#endif

src/ui/map3D/CheetahModel.cc

+#include "CheetahModel.h"
+
+#include "CheetahGL.h"
+
+CheetahModel::CheetahModel()
+ : cheetah_dl(-1)
+{
+
+}
+
+void
+CheetahModel::init(float red, float green, float blue)
+{
+	cheetah_dl = generatePixhawkCheetah(red, green, blue);
+}
+
+void
+CheetahModel::draw(void)
+{
+	glPushMatrix();
+        glScalef(0.5f, 0.5f, -0.5f);
+	glCallList(cheetah_dl);
+	glPopMatrix();
+}

src/ui/map3D/CheetahModel.h

+#ifndef CHEETAHMODEL_H_
+#define CHEETAHMODEL_H_
+
+#include <GL/gl.h>
+
+class CheetahModel
+{
+public:
+        CheetahModel();
+
+        void init(float red, float green, float blue);
+	void draw(void);
+
+private:
+	GLint cheetah_dl;
+};
+
+#endif /* CHEETAHMODEL_H_ */

src/ui/map3D/Q3DWidget.cc

+#include "Q3DWidget.h"
+
+#include <cmath>
+#include <GL/gl.h>
+#include <GL/glu.h>
+
+static const float KEY_ROTATE_AMOUNT = 5.0f;
+static const float KEY_MOVE_AMOUNT   = 10.0f;
+static const float KEY_ZOOM_AMOUNT   = 5.0f;
+
+Q3DWidget::Q3DWidget(QWidget* parent)
+  : QGLWidget(QGLFormat(QGL::Rgba | QGL::DoubleBuffer | QGL:: DepthBuffer |
+                        QGL::StencilBuffer), parent)
+  , userDisplayFunc(NULL)
+  , userKeyboardFunc(NULL)
+  , userMouseFunc(NULL)
+  , userMotionFunc(NULL)
+  , userDisplayFuncData(NULL)
+  , userKeyboardFuncData(NULL)
+  , userMouseFuncData(NULL)
+  , userMotionFuncData(NULL)
+  , windowWidth(0)
+  , windowHeight(0)
+  , requestedFps(0.0f)
+  , lastMouseX(0)
+  , lastMouseY(0)
+  , _is3D(true)
+  , _forceRedraw(false)
+  , allow2DRotation(true)
+  , limitCamera(false)
+  , timerFunc(NULL)
+  , timerFuncData(NULL)
+{
+	cameraPose.state = IDLE;
+	cameraPose.pan = 0.0f;
+	cameraPose.tilt = 180.0f;
+	cameraPose.distance = 10.0f;
+	cameraPose.xOffset = 0.0f;
+	cameraPose.yOffset = 0.0f;
+	cameraPose.zOffset = 0.0f;
+
+	cameraPose.xOffset2D = 0.0f;
+	cameraPose.yOffset2D = 0.0f;
+	cameraPose.rotation2D = 0.0f;
+	cameraPose.zoom = 1.0f;
+	cameraPose.warpX = 1.0f;
+	cameraPose.warpY = 1.0f;
+
+	cameraParams.zoomSensitivity = 0.05f;
+	cameraParams.rotateSensitivity = 0.5f;
+	cameraParams.moveSensitivity = 0.001f;
+	cameraParams.minZoomRange = 0.5f;
+	cameraParams.cameraFov = 30.0f;
+	cameraParams.minClipRange = 1.0f;
+	cameraParams.maxClipRange = 400.0f;
+	cameraParams.zoomSensitivity2D = 0.02f;
+	cameraParams.rotateSensitivity2D = 0.005f;
+	cameraParams.moveSensitivity2D = 1.0f;
+}
+
+Q3DWidget::~Q3DWidget()
+{
+
+}
+
+void
+Q3DWidget::initialize(int32_t windowX, int32_t windowY,
+                      int32_t windowWidth, int32_t windowHeight, float fps)
+{
+	this->windowWidth = windowWidth;
+	this->windowHeight = windowHeight;
+
+	requestedFps = fps;
+
+	resize(windowWidth, windowHeight);
+	move(windowX, windowY);
+
+	timer.start(static_cast<int>(floorf(1000.0f / requestedFps)), this);
+
+	_is3D = true;
+}
+
+void
+Q3DWidget::setCameraParams(float zoomSensitivity, float rotateSensitivity,
+							 float moveSensitivity, float minZoomRange,
+							 float cameraFov, float minClipRange,
+							 float maxClipRange)
+{
+	cameraParams.zoomSensitivity = zoomSensitivity;
+	cameraParams.rotateSensitivity = rotateSensitivity;
+	cameraParams.moveSensitivity = moveSensitivity;
+	cameraParams.minZoomRange = minZoomRange;
+	cameraParams.cameraFov = cameraFov;
+	cameraParams.minClipRange = minClipRange;
+	cameraParams.maxClipRange = maxClipRange;
+
+	limitCamera = true;
+	_forceRedraw = true;
+}
+
+void
+Q3DWidget::setCameraLimit(bool onoff)
+{
+	limitCamera = onoff;
+}
+
+void
+Q3DWidget::set2DCameraParams(float zoomSensitivity2D,
+                             float rotateSensitivity2D,
+                             float moveSensitivity2D)
+{
+	cameraParams.zoomSensitivity2D = zoomSensitivity2D;
+	cameraParams.rotateSensitivity2D = rotateSensitivity2D;
+	cameraParams.moveSensitivity2D = moveSensitivity2D;
+}
+
+void
+Q3DWidget::set3D(bool onoff)
+{
+	_is3D = onoff;
+}
+
+bool
+Q3DWidget::is3D(void) const
+{
+	return _is3D;
+}
+
+void
+Q3DWidget::setInitialCameraPos(float pan, float tilt, float range,
+								 float xOffset, float yOffset, float zOffset)
+{
+	cameraPose.pan = pan;
+	cameraPose.tilt = tilt;
+	cameraPose.distance = range;
+	cameraPose.xOffset = xOffset;
+	cameraPose.yOffset = yOffset;
+	cameraPose.zOffset = zOffset;
+}
+
+void
+Q3DWidget::setInitial2DCameraPos(float xOffset, float yOffset,
+								   float rotation, float zoom)
+{
+	cameraPose.xOffset2D = xOffset;
+	cameraPose.yOffset2D = yOffset;
+	cameraPose.rotation2D = rotation;
+	cameraPose.zoom = zoom;
+}
+
+void
+Q3DWidget::setCameraPose(const CameraPose& cameraPose)
+{
+	this->cameraPose = cameraPose;
+}
+
+CameraPose
+Q3DWidget::getCameraPose(void) const
+{
+	return cameraPose;
+}
+
+void
+Q3DWidget::setDisplayFunc(DisplayFunc func, void* clientData)
+{
+	userDisplayFunc = func;
+	userDisplayFuncData = clientData;
+}
+
+void
+Q3DWidget::setKeyboardFunc(KeyboardFunc func, void* clientData)
+{
+	userKeyboardFunc = func;
+	userKeyboardFuncData = clientData;
+}
+
+void
+Q3DWidget::setMouseFunc(MouseFunc func, void* clientData)
+{
+	userMouseFunc = func;
+	userMouseFuncData = clientData;
+}
+
+void
+Q3DWidget::setMotionFunc(MotionFunc func, void* clientData)
+{
+	userMotionFunc = func;
+	userMotionFuncData = clientData;
+}
+
+void
+Q3DWidget::addTimerFunc(uint32_t msecs, void(*func)(void *),
+                          void* clientData)
+{
+	timerFunc = func;
+	timerFuncData = clientData;
+
+	QTimer::singleShot(msecs, this, SLOT(userTimer()));
+}
+
+void
+Q3DWidget::userTimer(void)
+{
+	if (timerFunc)
+	{
+		timerFunc(timerFuncData);
+	}
+}
+
+void
+Q3DWidget::forceRedraw(void)
+{
+	_forceRedraw = true;
+}
+
+void
+Q3DWidget::set2DWarping(float warpX, float warpY)
+{
+	cameraPose.warpX = warpX;
+	cameraPose.warpY = warpY;
+}
+
+void
+Q3DWidget::recenter(void)
+{
+	cameraPose.xOffset = 0.0f;
+	cameraPose.yOffset = 0.0f;
+	cameraPose.zOffset = 0.0f;
+}
+
+void
+Q3DWidget::recenter2D(void)
+{
+	cameraPose.xOffset2D = 0.0f;
+	cameraPose.yOffset2D = 0.0f;
+}
+
+void
+Q3DWidget::set2DRotation(bool onoff)
+{
+	allow2DRotation = onoff;
+}
+
+std::pair<float,float>
+Q3DWidget::pickPoint(int32_t mouseX, int32_t mouseY)
+{
+	float cx = windowWidth / 2.0f;
+	float cy = windowHeight / 2.0f;
+	float pan = d2r(-90.0f - cameraPose.pan);
+	float tilt = d2r(90.0f - cameraPose.tilt);
+	float d = cameraPose.distance;
+	float f = cy / tanf(d2r(cameraParams.cameraFov / 2.0f));
+
+	float px = (mouseX - cx) * cosf(tilt) * d / (cosf(tilt) * f + sinf(tilt)
+				* mouseY - sinf(tilt) * cy);
+	float py = -(mouseY - cy) * d / (cosf(tilt) * f + sinf(tilt) * mouseY
+				- sinf(tilt) * cy);
+
+	std::pair<float,float> scene_coords;
+	scene_coords.first = px * cosf(pan) + py * sinf(pan) + cameraPose.xOffset;
+	scene_coords.second = -px * sinf(pan) + py * cosf(pan) + cameraPose.yOffset;
+
+	return scene_coords;
+}
+
+std::pair<float,float>
+Q3DWidget::get2DPosition(int32_t x, int32_t y)
+{
+	float dx = (x - windowWidth / 2.0f) / cameraPose.zoom;
+	float dy = (windowHeight / 2.0f - y) / cameraPose.zoom;
+	float ctheta = cosf(-cameraPose.rotation2D);
+	float stheta = sinf(-cameraPose.rotation2D);
+
+	std::pair<float,float> coords;
+	coords.first = cameraPose.xOffset2D + ctheta * dx - stheta * dy;
+	coords.second = cameraPose.yOffset2D + stheta * dx + ctheta * dy;
+
+	return coords;
+}
+
+int
+Q3DWidget::getWindowWidth(void)
+{
+	return windowWidth;
+}
+
+int
+Q3DWidget::getWindowHeight(void)
+{
+	return windowHeight;
+}
+
+int
+Q3DWidget::getLastMouseX(void)
+{
+	return lastMouseX;
+}
+
+int
+Q3DWidget::getLastMouseY(void)
+{
+	return lastMouseY;
+}
+
+int
+Q3DWidget::getMouseX(void)
+{
+	return mapFromGlobal(cursor().pos()).x();
+}
+
+int
+Q3DWidget::getMouseY(void)
+{
+	return mapFromGlobal(cursor().pos()).y();
+}
+
+
+void
+Q3DWidget::rotateCamera(float dx, float dy)
+{
+	cameraPose.pan += dx * cameraParams.rotateSensitivity;
+	cameraPose.tilt += dy * cameraParams.rotateSensitivity;
+	if (limitCamera)
+	{
+		if (cameraPose.tilt < 180.5f)
+		{
+			cameraPose.tilt = 180.5f;
+		}
+		else if (cameraPose.tilt > 269.5f)
+		{
+			cameraPose.tilt = 269.5f;
+		}
+	}
+}
+
+void
+Q3DWidget::zoomCamera(float dy)
+{
+	cameraPose.distance -=
+			dy * cameraParams.zoomSensitivity * cameraPose.distance;
+	if (cameraPose.distance < cameraParams.minZoomRange)
+	{
+		cameraPose.distance = cameraParams.minZoomRange;
+	}
+}
+
+void
+Q3DWidget::moveCamera(float dx, float dy)
+{
+	cameraPose.xOffset +=
+			-dy * cosf(d2r(cameraPose.pan)) * cameraParams.moveSensitivity
+			* cameraPose.distance;
+	cameraPose.yOffset +=
+			-dy * sinf(d2r(cameraPose.pan)) * cameraParams.moveSensitivity
+			* cameraPose.distance;
+	cameraPose.xOffset += dx * cosf(d2r(cameraPose.pan - 90.0f))
+			* cameraParams.moveSensitivity * cameraPose.distance;
+	cameraPose.yOffset += dx * sinf(d2r(cameraPose.pan - 90.0f))
+			* cameraParams.moveSensitivity * cameraPose.distance;
+}
+
+void
+Q3DWidget::rotateCamera2D(float dx)
+{
+	if (allow2DRotation)
+	{
+		cameraPose.rotation2D += dx * cameraParams.rotateSensitivity2D;
+	}
+}
+
+void
+Q3DWidget::zoomCamera2D(float dx)
+{
+	cameraPose.zoom += dx * cameraParams.zoomSensitivity2D * cameraPose.zoom;
+	if (cameraPose.zoom > 1e7f)
+	{
+		cameraPose.zoom = 1e7f;
+	}
+	if (cameraPose.zoom < 1e-7f)
+	{
+		cameraPose.zoom = 1e-7f;
+	}
+}
+
+void
+Q3DWidget::moveCamera2D(float dx, float dy)
+{
+	float scaledX = dx / cameraPose.zoom;
+	float scaledY = dy / cameraPose.zoom;
+
+	cameraPose.xOffset2D -= (scaledX * cosf(-cameraPose.rotation2D)
+			+ scaledY * sinf(-cameraPose.rotation2D)) / cameraPose.warpX
+			* cameraParams.moveSensitivity2D;
+	cameraPose.yOffset2D -= (scaledX * sinf(-cameraPose.rotation2D)
+			- scaledY * cosf(-cameraPose.rotation2D)) / cameraPose.warpY
+			* cameraParams.moveSensitivity2D;
+}
+
+void Q3DWidget::switchTo3DMode(void)
+{
+	// setup camera view
+	float cpan = d2r(cameraPose.pan);
+	float ctilt = d2r(cameraPose.tilt);
+	float cameraX = cameraPose.distance * cosf(cpan) * cosf(ctilt);
+	float cameraY = cameraPose.distance * sinf(cpan) * cosf(ctilt);
+	float cameraZ = cameraPose.distance * sinf(ctilt);
+	setDisplayMode3D(windowWidth, windowHeight);
+	glViewport(0, 0, static_cast<GLsizei>(windowWidth),
+			   static_cast<GLsizei>(windowHeight));
+	gluLookAt(cameraX + cameraPose.xOffset, cameraY + cameraPose.yOffset,
+			  cameraZ + cameraPose.zOffset, cameraPose.xOffset,
+			  cameraPose.yOffset, cameraPose.zOffset, 0.0, 0.0, 1.0);
+}
+
+void
+Q3DWidget::setDisplayMode2D(int32_t width, int32_t height)
+{
+	glDisable(GL_DEPTH_TEST);
+	glMatrixMode(GL_PROJECTION);
+	glLoadIdentity();
+	glOrtho(0.0, static_cast<GLfloat>(width), 0.0, static_cast<GLfloat>(height),
+			-10.0, 10.0);
+	glMatrixMode(GL_MODELVIEW);
+	glLoadIdentity();
+}
+
+void
+Q3DWidget::setDisplayMode3D(int32_t width, int32_t height)
+{
+	float aspect = static_cast<float>(width) / static_cast<float>(height);
+
+	glEnable(GL_DEPTH_TEST);
+	glMatrixMode(GL_PROJECTION);
+	glLoadIdentity();
+	gluPerspective(cameraParams.cameraFov, aspect,
+				   cameraParams.minClipRange, cameraParams.maxClipRange);
+	glMatrixMode(GL_MODELVIEW);
+	glLoadIdentity();
+	glScalef(-1.0f, -1.0f, 1.0f);
+}
+
+float
+Q3DWidget::r2d(float angle)
+{
+	return angle * 57.295779513082320876f;
+}
+
+float
+Q3DWidget::d2r(float angle)
+{
+	return angle * 0.0174532925199432957692f;
+}
+
+void
+Q3DWidget::initializeGL(void)
+{
+	float lightAmbient[] = {0.0f, 0.0f, 0.0f, 0.0f};
+	float lightDiffuse[] = {1.0f, 1.0f, 1.0f, 1.0f};
+	float lightSpecular[] = {1.0f, 1.0f, 1.0f, 1.0f};
+	float lightPosition[] = {0.0f, 0.0f, 100.0f, 0.0f};
+
+	glEnable(GL_DEPTH_TEST);
+	glShadeModel(GL_SMOOTH);
+	glLightfv(GL_LIGHT0, GL_AMBIENT, lightAmbient);
+	glLightfv(GL_LIGHT0, GL_DIFFUSE, lightDiffuse);
+	glLightfv(GL_LIGHT0, GL_SPECULAR, lightSpecular);
+	glLightfv(GL_LIGHT0, GL_POSITION, lightPosition);
+	glEnable(GL_LIGHT0);
+	glDisable(GL_LIGHTING);
+	glEnable(GL_NORMALIZE);
+}
+
+void
+Q3DWidget::paintGL(void)
+{
+	if (_is3D)
+	{
+		// setup camera view
+		switchTo3DMode();
+	}
+	else
+	{
+		setDisplayMode2D(windowWidth, windowHeight);
+		// do camera control
+		glTranslatef(static_cast<float>(windowWidth) / 2.0f,
+					 static_cast<float>(windowHeight) / 2.0f,
+					 0.0f);
+		glScalef(cameraPose.zoom, cameraPose.zoom, 1.0f);
+		glRotatef(r2d(cameraPose.rotation2D), 0.0f, 0.0f, 1.0f);
+		glScalef(cameraPose.warpX, cameraPose.warpY, 1.0f);
+		glTranslatef(-cameraPose.xOffset2D, -cameraPose.yOffset2D, 0.0f);
+	}
+
+	// turn on smooth lines
+	glEnable(GL_BLEND);
+	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+	glEnable(GL_LINE_SMOOTH);
+
+        glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
+	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+	glLineWidth(1.0f);
+
+	if (userDisplayFunc)
+	{
+		userDisplayFunc(userDisplayFuncData);
+	}
+	glFlush();
+}
+
+void
+Q3DWidget::resizeGL(int32_t width, int32_t height)
+{
+	glViewport(0, 0, width, height);
+
+	windowWidth = width;
+	windowHeight = height;
+
+	if (_is3D)
+	{
+		setDisplayMode3D(width, height);
+	}
+	else
+	{
+		setDisplayMode2D(width, height);
+	}
+}
+
+void
+Q3DWidget::keyPressEvent(QKeyEvent* event)
+{
+	float dx = 0.0f, dy = 0.0f;
+
+	Qt::KeyboardModifiers modifiers = event->modifiers();
+	if (_is3D)
+	{
+		if (modifiers & Qt::ControlModifier)
+		{
+			switch (event->key())
+			{
+			case Qt::Key_Left:
+				dx = -KEY_ROTATE_AMOUNT;
+				dy = 0.0f;
+				break;
+			case Qt::Key_Right:
+				dx = KEY_ROTATE_AMOUNT;
+				dy = 0.0f;
+				break;
+			case Qt::Key_Up:
+				dx = 0.0f;
+				dy = KEY_ROTATE_AMOUNT;
+				break;
+			case Qt::Key_Down:
+				dx = 0.0f;
+				dy = -KEY_ROTATE_AMOUNT;
+				break;
+			default:
+				QWidget::keyPressEvent(event);
+			}
+			if (dx != 0.0f || dy != 0.0f)
+			{
+				rotateCamera(dx, dy);
+			}
+		}
+		else if (modifiers & Qt::AltModifier)
+		{
+			switch (event->key())
+			{
+			case Qt::Key_Up:
+				dy = KEY_ZOOM_AMOUNT;
+				break;
+			case Qt::Key_Down:
+				dy = -KEY_ZOOM_AMOUNT;
+				break;
+			default:
+				QWidget::keyPressEvent(event);
+			}
+			if (dy != 0.0f)
+			{
+				zoomCamera(dy);
+			}
+		}
+		else
+		{
+			switch (event->key())
+			{
+			case Qt::Key_Left:
+				dx = KEY_MOVE_AMOUNT;
+				dy = 0.0f;
+				break;
+			case Qt::Key_Right:
+				dx = -KEY_MOVE_AMOUNT;
+				dy = 0.0f;
+				break;
+			case Qt::Key_Up:
+				dx = 0.0f;
+				dy = -KEY_MOVE_AMOUNT;
+				break;
+			case Qt::Key_Down:
+				dx = 0.0f;
+				dy = KEY_MOVE_AMOUNT;
+				break;
+			default:
+				QWidget::keyPressEvent(event);
+			}
+			if (dx != 0.0f || dy != 0.0f)
+			{
+				moveCamera(dx, dy);
+			}
+		}
+	}
+	else {
+		if (modifiers & Qt::ControlModifier)
+		{
+			switch (event->key())
+			{
+			case Qt::Key_Left:
+				dx = KEY_ROTATE_AMOUNT;
+				dy = 0.0f;
+				break;
+			case Qt::Key_Right:
+				dx = -KEY_ROTATE_AMOUNT;
+				dy = 0.0f;
+				break;
+			default:
+				QWidget::keyPressEvent(event);
+			}
+			if (dx != 0.0f)
+			{
+				rotateCamera2D(dx);
+			}
+		}
+		else if (modifiers & Qt::AltModifier)
+		{
+			switch (event->key())
+			{
+			case Qt::Key_Up:
+				dy = KEY_ZOOM_AMOUNT;
+				break;
+			case Qt::Key_Down:
+				dy = -KEY_ZOOM_AMOUNT;
+				break;
+			default:
+				QWidget::keyPressEvent(event);
+			}
+			if (dy != 0.0f)
+			{
+				zoomCamera2D(dy);
+			}
+		}
+		else {
+			switch (event->key())
+			{
+			case Qt::Key_Left:
+				dx = KEY_MOVE_AMOUNT;
+				dy = 0.0f;
+				break;
+			case Qt::Key_Right:
+				dx = -KEY_MOVE_AMOUNT;
+				dy = 0.0f;
+				break;
+			case Qt::Key_Up:
+				dx = 0.0f;
+				dy = KEY_MOVE_AMOUNT;
+				break;
+			case Qt::Key_Down:
+				dx = 0.0f;
+				dy = -KEY_MOVE_AMOUNT;
+				break;
+			default:
+				QWidget::keyPressEvent(event);
+			}
+			if (dx != 0.0f || dy != 0.0f)
+			{
+				moveCamera2D(dx, dy);
+			}
+		}
+	}
+
+	_forceRedraw = true;
+
+	if (userKeyboardFunc)
+	{
+		if (event->text().isEmpty())
+		{
+			userKeyboardFunc(0, userKeyboardFuncData);
+		}
+		else
+		{
+			userKeyboardFunc(event->text().at(0).toAscii(),
+							 userKeyboardFuncData);
+		}
+	}
+}
+
+void
+Q3DWidget::mousePressEvent(QMouseEvent* event)
+{
+	Qt::KeyboardModifiers modifiers = event->modifiers();
+
+	if (!(modifiers & (Qt::ControlModifier | Qt::AltModifier)))
+	{
+		lastMouseX = event->x();
+		lastMouseY = event->y();
+		if (event->button() == Qt::LeftButton)
+		{
+			cameraPose.state = ROTATING;
+		}
+		else if (event->button() == Qt::MidButton)
+		{
+			cameraPose.state = MOVING;
+		}
+		else if (event->button() == Qt::RightButton)
+		{
+			cameraPose.state = ZOOMING;
+		}
+	}
+
+	_forceRedraw = true;
+
+	if (userMouseFunc)
+	{
+		userMouseFunc(event->button(), MOUSE_STATE_DOWN, event->x(), event->y(),
+					  userMouseFuncData);
+	}
+}
+
+void
+Q3DWidget::mouseReleaseEvent(QMouseEvent* event)
+{
+	Qt::KeyboardModifiers modifiers = event->modifiers();
+
+	if (!(modifiers & (Qt::ControlModifier | Qt::AltModifier)))
+	{
+		cameraPose.state = IDLE;
+	}
+
+	_forceRedraw = true;
+
+	if (userMouseFunc)
+	{
+		userMouseFunc(event->button(), MOUSE_STATE_UP, event->x(), event->y(),
+					  userMouseFuncData);
+	}
+}
+
+void
+Q3DWidget::mouseMoveEvent(QMouseEvent* event)
+{
+	int32_t dx = event->x() - lastMouseX;
+	int32_t dy = event->y() - lastMouseY;
+
+	if (_is3D)
+	{
+		if (cameraPose.state == ROTATING)
+		{
+			rotateCamera(static_cast<float>(dx), static_cast<float>(dy));
+		}
+		else if (cameraPose.state == MOVING)
+		{
+			moveCamera(static_cast<float>(dx), static_cast<float>(dy));
+		}
+		else if (cameraPose.state == ZOOMING)
+		{
+			zoomCamera(static_cast<float>(dy));
+		}
+	}
+	else
+	{
+		if (cameraPose.state == ROTATING)
+		{
+			if (event->x() > windowWidth / 2)
+			{
+				dy *= -1;
+			}
+
+			rotateCamera2D(static_cast<float>(dx));
+		}
+		else if (cameraPose.state == MOVING)
+		{
+			moveCamera2D(static_cast<float>(dx), static_cast<float>(dy));
+		}
+		else if (cameraPose.state == ZOOMING)
+		{
+			zoomCamera2D(static_cast<float>(dy));
+		}
+	}
+
+	lastMouseX = event->x();
+	lastMouseY = event->y();
+	_forceRedraw = true;
+
+	if (userMotionFunc)
+	{
+		userMotionFunc(event->x(), event->y(), userMotionFuncData);
+	}
+}
+
+void
+Q3DWidget::wheelEvent(QWheelEvent* event)
+{
+	if (_is3D)
+	{
+		zoomCamera(static_cast<float>(event->delta()) / 40.0f);
+	}
+	else
+	{
+		zoomCamera2D(static_cast<float>(event->delta()) / 40.0f);
+	}
+
+	_forceRedraw = true;
+}
+
+void
+Q3DWidget::timerEvent(QTimerEvent* event)
+{
+	if (event->timerId() == timer.timerId())
+	{
+		if (_forceRedraw)
+		{
+			updateGL();
+			_forceRedraw = false;
+		}
+	}
+	else
+	{
+		QObject::timerEvent(event);
+	}
+}
+
+void
+Q3DWidget::closeEvent(QCloseEvent *)
+{
+	// exit application
+}

src/ui/map3D/Q3DWidget.h

+#ifndef Q3DWIDGET_H_
+#define Q3DWIDGET_H_
+
+#include <boost/scoped_ptr.hpp>
+#include <inttypes.h>
+#include <string>
+#include <QtOpenGL>
+#include <QtGui>
+
+enum CameraState
+{
+	IDLE = 0,
+	ROTATING = 1,
+	MOVING = 2,
+	ZOOMING = 3
+};
+
+struct CameraPose
+{
+        CameraState state;
+	float pan, tilt, distance;
+	float xOffset, yOffset, zOffset;
+	float xOffset2D, yOffset2D, rotation2D, zoom, warpX, warpY;
+};
+
+struct CameraParams
+{
+	float zoomSensitivity;
+	float rotateSensitivity;
+	float moveSensitivity;
+
+	float minZoomRange;
+	float cameraFov;
+	float minClipRange;
+	float maxClipRange;
+
+	float zoomSensitivity2D;
+	float rotateSensitivity2D;
+	float moveSensitivity2D;
+};
+
+enum MouseState
+{
+	MOUSE_STATE_UP = 0,
+	MOUSE_STATE_DOWN = 1
+};
+
+typedef void (*DisplayFunc)(void *);
+typedef void (*KeyboardFunc)(char, void *);
+typedef void (*MouseFunc)(Qt::MouseButton, MouseState, int32_t, int32_t, void *);
+typedef void (*MotionFunc)(int32_t, int32_t, void *);
+
+class Q3DWidget: public QGLWidget
+{
+	Q_OBJECT
+
+public:
+        explicit Q3DWidget(QWidget* parent);
+        ~Q3DWidget();
+
+        void initialize(int32_t windowX, int32_t windowY,
+                        int32_t windowWidth, int32_t windowHeight, float fps);
+
+	void setCameraParams(float zoomSensitivity, float rotateSensitivity,
+						 float moveSensitivity, float minZoomRange,
+						 float camera_fov, float minClipRange,
+						 float maxClipRange);
+
+	void setCameraLimit(bool onoff);
+
+	void set2DCameraParams(float zoomSensitivity,
+						   float rotateSensitivity,
+						   float moveSensitivity);
+
+	void set3D(bool onoff);
+	bool is3D(void) const;
+
+	void setInitialCameraPos(float pan, float tilt, float range,
+							 float xOffset, float yOffset, float zOffset);
+	void setInitial2DCameraPos(float xOffset, float yOffset,
+							   float rotation, float zoom);
+        void setCameraPose(const CameraPose& cameraPose);
+        CameraPose getCameraPose(void) const;
+
+        void setDisplayFunc(DisplayFunc func, void* clientData);
+        void setKeyboardFunc(KeyboardFunc func, void* clientData);
+        void setMouseFunc(MouseFunc func, void* clientData);
+        void setMotionFunc(MotionFunc func, void* clientData);
+	void addTimerFunc(uint32_t msecs, void(*func)(void *),
+					  void* clientData);
+
+	void forceRedraw(void);
+
+	void set2DWarping(float warpX, float warpY);
+
+	void recenter(void);
+	void recenter2D(void);
+
+	void set2DRotation(bool onoff);
+
+	std::pair<float,float> pickPoint(int32_t mouseX, int32_t mouseY);
+
+	std::pair<float,float> get2DPosition(int32_t x, int32_t y);
+
+	int32_t getWindowWidth(void);
+	int32_t getWindowHeight(void);
+	int32_t getLastMouseX(void);
+	int32_t getLastMouseY(void);
+	int32_t getMouseX(void);
+	int32_t getMouseY(void);
+
+private Q_SLOTS:
+	void userTimer(void);
+
+private:
+	void rotateCamera(float dx, float dy);
+	void zoomCamera(float dy);
+	void moveCamera(float dx, float dy);
+	void rotateCamera2D(float dx);
+	void zoomCamera2D(float dx);
+	void moveCamera2D(float dx, float dy);
+
+	void switchTo3DMode(void);
+	void setDisplayMode2D(int32_t width, int32_t height);
+	void setDisplayMode3D(int32_t width, int32_t height);
+
+	float r2d(float angle);
+	float d2r(float angle);
+
+	// QGLWidget events
+	void initializeGL(void);
+	void paintGL(void);
+	void resizeGL(int32_t width, int32_t height);
+
+	// Qt events
+	void keyPressEvent(QKeyEvent* event);
+	void mousePressEvent(QMouseEvent* event);
+	void mouseReleaseEvent(QMouseEvent* event);
+	void mouseMoveEvent(QMouseEvent* event);
+	void wheelEvent(QWheelEvent *wheel);
+	void timerEvent(QTimerEvent* event);
+	void closeEvent(QCloseEvent* event);
+
+        DisplayFunc userDisplayFunc;
+        KeyboardFunc userKeyboardFunc;
+        MouseFunc userMouseFunc;
+        MotionFunc userMotionFunc;
+
+	void* userDisplayFuncData;
+	void* userKeyboardFuncData;
+	void* userMouseFuncData;
+	void* userMotionFuncData;
+
+	int32_t windowWidth, windowHeight;
+	float requestedFps;
+        CameraPose cameraPose;
+	int32_t lastMouseX, lastMouseY;
+
+	bool _is3D;
+
+	bool _forceRedraw;
+	bool allow2DRotation;
+	bool limitCamera;
+
+        CameraParams cameraParams;
+
+	QBasicTimer timer;
+
+	void (*timerFunc)(void *);
+	void* timerFuncData;
+};
+
+#endif

src/ui/map3D/QMap3DWidget.cc

+#include "QMap3DWidget.h"
+#include <QPushButton>
+
+#include <sys/time.h>
+
+#include "UASManager.h"
+#include "UASInterface.h"
+
+QMap3DWidget::QMap3DWidget(QWidget* parent)
+     : Q3DWidget(parent),
+     uas(NULL),
+     lastRedrawTime(0.0)
+{
+    setFocusPolicy(Qt::StrongFocus);
+
+    initialize(10, 10, 1000, 900, 10.0f);
+    setCameraParams(0.05f, 0.5f, 0.001f, 0.5f, 30.0f, 0.01f, 400.0f);
+
+    setDisplayFunc(display, this);
+    addTimerFunc(100, timer, this);
+
+    QPushButton* mapButton = new QPushButton(this);
+    mapButton->setText("Grid");
+
+    // display the MapControl in the application
+    QGridLayout* layout = new QGridLayout(this);
+    layout->setMargin(0);
+    layout->setSpacing(2);
+    //layout->addWidget(mc, 0, 0, 1, 2);
+    layout->addWidget(mapButton, 1, 0);
+    layout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 1);
+    layout->setRowStretch(0, 100);
+    layout->setRowStretch(1, 1);
+    layout->setColumnStretch(0, 1);
+    layout->setColumnStretch(1, 50);
+    setLayout(layout);
+
+    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
+            this, SLOT(setActiveUAS(UASInterface*)));
+}
+
+QMap3DWidget::~QMap3DWidget()
+{
+
+}
+
+void
+QMap3DWidget::display(void* clientData)
+{
+    QMap3DWidget* map3d = reinterpret_cast<QMap3DWidget *>(clientData);
+    map3d->displayHandler();
+}
+
+void
+QMap3DWidget::displayHandler(void)
+{
+    if (cheetahModel.get() == 0)
+    {
+        cheetahModel.reset(new CheetahModel);
+        cheetahModel->init(1.0f, 1.0f, 1.0f);
+    }
+
+    if (uas == NULL)
+    {
+        return;
+    }
+
+    // turn on smooth lines
+    glEnable(GL_LINE_SMOOTH);
+    glHint(GL_LINE_SMOOTH_HINT, GL_NICEST);
+    glEnable(GL_BLEND);
+    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+    // clear window
+    glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
+    glClear(GL_COLOR_BUFFER_BIT);
+
+    // draw Cheetah model
+    glPushMatrix();
+
+    glRotatef(uas->getYaw(), 0.0f, 0.0f, 1.0f);
+    glRotatef(uas->getPitch(), 0.0f, 1.0f, 0.0f);
+    glRotatef(uas->getRoll(), 1.0f, 0.0f, 0.0f);
+
+    glLineWidth(3.0f);
+    glColor3f(0.0f, 1.0f, 0.0f);
+    glBegin(GL_LINES);
+    glVertex3f(0.0f, 0.0f, 0.0f);
+    glVertex3f(0.3f, 0.0f, 0.0f);
+    glEnd();
+
+    cheetahModel->draw();
+
+    glPopMatrix();
+}
+
+/**
+ *
+ * @param uas the UAS/MAV to monitor/display with the HUD
+ */
+void QMap3DWidget::setActiveUAS(UASInterface* uas)
+{
+    if (this->uas != NULL && this->uas != uas)
+    {
+        // Disconnect any previously connected active MAV
+        //disconnect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64)));
+    }
+
+    this->uas = uas;
+}
+
+void
+QMap3DWidget::timer(void* clientData)
+{
+    QMap3DWidget* map3d = reinterpret_cast<QMap3DWidget *>(clientData);
+    map3d->timerHandler();
+}
+
+void
+QMap3DWidget::timerHandler(void)
+{
+    double timeLapsed = getTime() - lastRedrawTime;
+    if (timeLapsed > 0.1)
+    {
+        forceRedraw();
+        lastRedrawTime = getTime();
+    }
+    addTimerFunc(100, timer, this);
+}
+
+double
+QMap3DWidget::getTime(void) const
+{
+     struct timeval tv;
+
+     gettimeofday(&tv, NULL);
+
+     return static_cast<double>(tv.tv_sec) +
+             static_cast<double>(tv.tv_usec) / 1000000.0;
+}

src/ui/map3D/QMap3DWidget.h

+#ifndef QMAP3DWIDGET_H
+#define QMAP3DWIDGET_H
+
+#include "Q3DWidget.h"
+#include "CheetahModel.h"
+
+class UASInterface;
+
+class QMap3DWidget : public Q3DWidget
+{
+    Q_OBJECT
+
+public:
+    explicit QMap3DWidget(QWidget* parent);
+    ~QMap3DWidget();
+
+    static void display(void* clientData);
+    void displayHandler(void);
+
+    static void timer(void* clientData);
+    void timerHandler(void);
+
+    double getTime(void) const;
+
+public slots:
+    void setActiveUAS(UASInterface* uas);
+
+protected:
+    UASInterface* uas;
+
+private:
+    double lastRedrawTime;
+
+    boost::scoped_ptr<CheetahModel> cheetahModel;
+};
+
+#endif // QMAP3DWIDGET_H