Added support for OSG on Mac platform

qgroundcontrol.pri

@@ -34,9 +34,17 @@ release {
 #    DEFINES += QT_NO_WARNING_OUTPUT
 }
 
+QMAKE_PRE_LINK += echo "Copying files"
+
+#QMAKE_PRE_LINK += && cp -rf $$BASEDIR/models $$TARGETDIR/debug/.
+#QMAKE_PRE_LINK += && cp -rf $$BASEDIR/models $$TARGETDIR/release/.
+
 # MAC OS X
 macx { 
 
+    COMPILER_VERSION = $$system(gcc -v)
+    message(Using compiler $$COMPILER_VERSION)
+
     HARDWARE_PLATFORM = $$system(uname -a)
     contains( HARDWARE_PLATFORM, 9.6.0 ) || contains( HARDWARE_PLATFORM, 9.7.0 ) || contains( HARDWARE_PLATFORM, 9.8.0 ) || contains( HARDWARE_PLATFORM, 9.9.0 ) {
         # x86 Mac OS X Leopard 10.5 and earlier
@@ -55,18 +63,15 @@ macx {
         message(Building for Mac OS X 64bit/Snow Leopard 10.6 and later)
     }
 
-    QMAKE_MACOSX_DEPLOYMENT_TARGET = 10.6
+    QMAKE_MACOSX_DEPLOYMENT_TARGET = 10.5
 
     DESTDIR = $$BASEDIR/bin/mac
-    INCLUDEPATH += -framework SDL \
-        $$BASEDIR/../mavlink/contrib/slugs/include \
-        $$BASEDIR/../mavlink/include
+    INCLUDEPATH += -framework SDL
 
     LIBS += -framework IOKit \
         -framework SDL \
         -framework CoreFoundation \
         -framework ApplicationServices \
- #       -framework GLUT \
         -lm
     
     ICON = $$BASEDIR/images/icons/macx.icns
@@ -74,24 +79,67 @@ macx {
     # Copy audio files if needed
     QMAKE_PRE_LINK += cp -rf $$BASEDIR/audio $$DESTDIR/qgroundcontrol.app/Contents/MacOs/.
 
-    exists(/opt/local/lib/osg):exists("/opt/local/lib/osgEarth") {
-    message("Building support for OSGEARTH")
-    DEPENDENCIES_PRESENT += osgearth
-    LIBS += -L/opt/local/lib/
-    INCLUDEPATH += /opt/local/include
-    # Include OpenSceneGraph and osgEarth libraries
-    LIBS += -losg \
-        -losgViewer \
-        -losgEarth \
-        -losgEarthUtil
+    exists(/Library/Frameworks/osg.framework):exists(/Library/Frameworks/OpenThreads.framework) {
+    # No check for GLUT.framework since it's a MAC default
+    message("Building support for OpenSceneGraph")
+    DEPENDENCIES_PRESENT += osg
     DEFINES += QGC_OSG_ENABLED
+    # Include OpenSceneGraph libraries
+    INCLUDEPATH += -framework GLUT \
+            -framework Carbon \
+            -framework OpenThreads \
+            -framework osg \
+            -framework osgViewer \
+            -framework osgGA \
+            -framework osgDB \
+            -framework osgText \
+            -framework osgWidget
+
+    LIBS += -framework GLUT \
+            -framework Carbon \
+            -framework OpenThreads \
+            -framework osg \
+            -framework osgViewer \
+            -framework osgGA \
+            -framework osgDB \
+            -framework osgText \
+            -framework osgWidget
+    }
+
+    exists(/usr/include/osgEarth) {
+    message("Building support for osgEarth")
+    DEPENDENCIES_PRESENT += osgearth
+    # Include osgEarth libraries
+    INCLUDEPATH += -framework GDAL \
+            $$IN_PWD/lib/mac32-gcc/include \
+            -framework GEOS \
+            -framework SQLite3 \
+            -framework osgFX \
+            -framework osgTerrain
+
+    LIBS += -framework GDAL \
+            -framework GEOS \
+            -framework SQLite3 \
+            -framework osgFX \
+            -framework osgTerrain \
+    DEFINES += QGC_OSGEARTH_ENABLED
     }
+
+    exists(/opt/local/include/libfreenect) {
+    message("Building support for libfreenect")
+    DEPENDENCIES_PRESENT += libfreenect
+    # Include libfreenect libraries
+    LIBS += -lfreenect
+    DEFINES += QGC_LIBFREENECT_ENABLED
+    }
+
+    # osg/osgEarth dynamic casts might fail without this compiler option.
+    # see http://osgearth.org/wiki/FAQ for details.
+    #QMAKE_CXXFLAGS += -Wl, -E
 }
 
 # GNU/Linux
 linux-g++ {
-
-    CONFIG += debug
     
     debug {
         DESTDIR = $$BUILDDIR/debug
@@ -131,21 +179,41 @@ linux-g++ {
         -lSDL \
         -lSDLmain
 
-exists(/usr/include/osgEarth) | exists(/usr/local/include/osgEarth) {
-message("Building support for OSGEARTH")
-DEPENDENCIES_PRESENT += osgearth
-# Include OpenSceneGraph and osgEarth libraries
-LIBS += -losg \
-    -losgViewer \
-    -losgEarth \
-    -losgEarthUtil
-DEFINES += QGC_OSG_ENABLED
-}
+    exists(/usr/include/osg) {
+    message("Building support for OpenSceneGraph")
+    DEPENDENCIES_PRESENT += osg
+    # Include OpenSceneGraph libraries
+    LIBS += -losg
+    DEFINES += QGC_OSG_ENABLED
+    }
 
-QMAKE_CXXFLAGS += -Wl,-E
+    exists(/usr/include/osgEarth) | exists(/usr/local/include/osgEarth) {
+    message("Building support for osgEarth")
+    DEPENDENCIES_PRESENT += osgearth
+    # Include osgEarth libraries
+    LIBS += -losgViewer \
+            -losgEarth \
+            -losgEarthUtil
+    DEFINES += QGC_OSGEARTH_ENABLED
+    }
+
+    exists(/usr/local/include/libfreenect) {
+    message("Building suplocport for libfreenect")
+    DEPENDENCIES_PRESENT += libfreenect
+    INCLUDEPATH += /usr/include/libusb-1.0
+    # Include libfreenect libraries
+    LIBS += -lfreenect
+    DEFINES += QGC_LIBFREENECT_ENABLED
+    }
 
-        #-lflite_cmu_us_rms \
-        #-lflite_cmu_us_slt \
+    QMAKE_PRE_LINK += && cp -rf $$BASEDIR/models $$TARGETDIR/debug/.
+    QMAKE_PRE_LINK += && cp -rf $$BASEDIR/models $$TARGETDIR/release/.
+    QMAKE_PRE_LINK += && cp -rf $$BASEDIR/data $$TARGETDIR/debug/.
+    QMAKE_PRE_LINK += && cp -rf $$BASEDIR/data $$TARGETDIR/release/.
+
+    # osg/osgEarth dynamic casts might fail without this compiler option.
+    # see http://osgearth.org/wiki/FAQ for details.
+    QMAKE_CXXFLAGS += -Wl, -E
 }
 
 linux-g++-64 {
@@ -188,16 +256,36 @@ linux-g++-64 {
         -lSDL \
         -lSDLmain
 
-exists(/usr/lib/osg):exists(/usr/lib/osgEarth) {
-message("Building support for OSGEARTH")
-DEPENDENCIES_PRESENT += osgearth
-# Include OpenSceneGraph and osgEarth libraries
-LIBS += -losg \
-    -losgViewer \
-    -losgEarth
-DEFINES += QGC_OSG_ENABLED
-}
+    exists(/usr/include/osg) {
+    message("Building support for OpenSceneGraph")
+    DEPENDENCIES_PRESENT += osg
+    # Include OpenSceneGraph libraries
+    LIBS += -losg
+    DEFINES += QGC_OSG_ENABLED
+    }
+
+    exists(/usr/include/osgEarth) {
+    message("Building support for osgEarth")
+    DEPENDENCIES_PRESENT += osgearth
+    # Include osgEarth libraries
+    LIBS += -losgViewer \
+            -losgEarth \
+            -losgEarthUtil
+    DEFINES += QGC_OSGEARTH_ENABLED
+    }
+
+    exists(/usr/local/include/libfreenect) {
+    message("Building support for libfreenect")
+    DEPENDENCIES_PRESENT += libfreenect
+    INCLUDEPATH += /usr/include/libusb-1.0
+    # Include libfreenect libraries
+    LIBS += -lfreenect
+    DEFINES += QGC_LIBFREENECT_ENABLED
+    }
 
+    # osg/osgEarth dynamic casts might fail without this compiler option.
+    # see http://osgearth.org/wiki/FAQ for details.
+    QMAKE_CXXFLAGS += -Wl, -E
 }
 
 # Windows (32bit)
@@ -222,7 +310,13 @@ win32-msvc2008 {
     QMAKE_PRE_LINK += cp -f $$BASEDIR/lib/sdl/win32/SDL.dll $$TARGETDIR/debug/. &&
     QMAKE_PRE_LINK += cp -f $$BASEDIR/lib/sdl/win32/SDL.dll $$TARGETDIR/release/. &&
     QMAKE_PRE_LINK += cp -rf $$BASEDIR/audio $$TARGETDIR/debug/. &&
-    QMAKE_PRE_LINK += cp -rf $$BASEDIR/audio $$TARGETDIR/release/.
+    QMAKE_PRE_LINK += cp -rf $$BASEDIR/audio $$TARGETDIR/release/. &&
+    QMAKE_PRE_LINK += cp -rf $$BASEDIR/models $$TARGETDIR/debug/. &&
+    QMAKE_PRE_LINK += cp -rf $$BASEDIR/models $$TARGETDIR/release/.
+
+    # osg/osgEarth dynamic casts might fail without this compiler option.
+    # see http://osgearth.org/wiki/FAQ for details.
+    QMAKE_CXXFLAGS += /Wl /E
 }
 
 # Windows (32bit)
@@ -255,8 +349,14 @@ win32-g++ {
     # Copy dependencies
     QMAKE_PRE_LINK += cp -f $$BASEDIR/lib/sdl/win32/SDL.dll $$BUILDDIR/debug/. &&
     QMAKE_PRE_LINK += cp -f $$BASEDIR/lib/sdl/win32/SDL.dll $$BUILDDIR/release/. &&
-    QMAKE_PRE_LINK += cp -rf $$BASEDIR/audio $$BUILDDIR/debug/. &&
-    QMAKE_PRE_LINK += cp -rf $$BASEDIR/audio $$BUILDDIR/release/.
+    QMAKE_PRE_LINK += cp -rf $$BASEDIR/audio $$TARGETDIR/debug/. &&
+    QMAKE_PRE_LINK += cp -rf $$BASEDIR/audio $$TARGETDIR/release/.
+    QMAKE_PRE_LINK += cp -rf $$BASEDIR/models $$TARGETDIR/debug/. &&
+    QMAKE_PRE_LINK += cp -rf $$BASEDIR/models $$TARGETDIR/release/.
+
+    # osg/osgEarth dynamic casts might fail without this compiler option.
+    # see http://osgearth.org/wiki/FAQ for details.
+    QMAKE_CXXFLAGS += -Wl, -E
 }
 
 # Windows (64bit)
@@ -291,5 +391,8 @@ win64-g++ {
     QMAKE_PRE_LINK += cp -f $$BASEDIR/lib/sdl/win32/SDL.dll $$BUILDDIR/release/. &&
     QMAKE_PRE_LINK += cp -rf $$BASEDIR/audio $$BUILDDIR/debug/. &&
     QMAKE_PRE_LINK += cp -rf $$BASEDIR/audio $$BUILDDIR/release/.
-}
 
+    # osg/osgEarth dynamic casts might fail without this compiler option.
+    # see http://osgearth.org/wiki/FAQ for details.
+    QMAKE_CXXFLAGS += -Wl, -E
+}

qgroundcontrol.pro

@@ -231,18 +231,35 @@ HEADERS += src/MG.h \
     src/ui/RadioCalibration/CurveCalibrator.h \
     src/ui/RadioCalibration/AbstractCalibrator.h \
     src/comm/QGCMAVLink.h
-contains(DEPENDENCIES_PRESENT, osgearth) { 
-    message("Including headers for OSGEARTH")
+
+
+contains(DEPENDENCIES_PRESENT, osg) { 
+    message("Including headers for OpenSceneGraph")
     
     # Enable only if OpenSceneGraph is available
     HEADERS += src/ui/map3D/Q3DWidget.h \
+        src/ui/map3D/GCManipulator.h \
+        src/ui/map3D/ImageWindowGeode.h \
         src/ui/map3D/QOSGWidget.h \
-        src/ui/map3D/QMap3D.h \
         src/ui/map3D/PixhawkCheetahGeode.h \
         src/ui/map3D/Pixhawk3DWidget.h \
-        src/ui/map3D/Q3DWidgetFactory.h \
-        src/ui/map3D/GCManipulator.h
+        src/ui/map3D/Q3DWidgetFactory.h
+
+contains(DEPENDENCIES_PRESENT, osgearth) { 
+    message("Including headers for OSGEARTH")
+    
+    # Enable only if OpenSceneGraph is available
+    HEADERS += src/ui/map3D/QMap3D.h
+}
 }
+
+contains(DEPENDENCIES_PRESENT, libfreenect) { 
+    message("Including headers for libfreenect")
+    
+    # Enable only if libfreenect is available
+    HEADERS += src/input/Freenect.h
+}
+
 SOURCES += src/main.cc \
     src/Core.cc \
     src/uas/UASManager.cc \
@@ -311,19 +328,39 @@ SOURCES += src/main.cc \
     src/ui/RadioCalibration/SwitchCalibrator.cc \
     src/ui/RadioCalibration/CurveCalibrator.cc \
     src/ui/RadioCalibration/AbstractCalibrator.cc \
-    src/ui/RadioCalibration/RadioCalibrationData.cc # src/ui/WaypointGlobalView.cc \
-contains(DEPENDENCIES_PRESENT, osgearth) { 
-    message("Including sources for OSGEARTH")
+    src/ui/RadioCalibration/RadioCalibrationData.cc
+
+contains(DEPENDENCIES_PRESENT, osg) { 
+    message("Including sources for OpenSceneGraph")
     
     # Enable only if OpenSceneGraph is available
     SOURCES += src/ui/map3D/Q3DWidget.cc \
-        src/ui/map3D/QOSGWidget.cc \
-        src/ui/map3D/QMap3D.cc \
+    src/ui/map3D/ImageWindowGeode.cc \
+    src/ui/map3D/GCManipulator.cc \
+    src/ui/map3D/QOSGWidget.cc \
         src/ui/map3D/PixhawkCheetahGeode.cc \
         src/ui/map3D/Pixhawk3DWidget.cc \
-        src/ui/map3D/Q3DWidgetFactory.cc \
-        src/ui/map3D/GCManipulator.cc
+        src/ui/map3D/Q3DWidgetFactory.cc
+
+
+
+contains(DEPENDENCIES_PRESENT, osgearth) { 
+    message("Including sources for osgEarth")
+    
+    # Enable only if OpenSceneGraph is available
+    SOURCES += src/ui/map3D/QMap3D.cc
+
+}
 }
+
+contains(DEPENDENCIES_PRESENT, libfreenect) { 
+    message("Including sources for libfreenect")
+    
+    # Enable only if libfreenect is available
+    SOURCES += src/input/Freenect.cc
+}
+
+
 RESOURCES += mavground.qrc
 
 # Include RT-LAB Library

src/input/Freenect.cc

+#include "Freenect.h"
+
+#include <cmath>
+#include <string.h>
+#include <QDebug>
+
+Freenect::Freenect()
+    : context(NULL)
+    , device(NULL)
+    , tiltAngle(0)
+{
+    for (int i = 0; i < 2048; ++i)
+    {
+        float v = static_cast<float>(i) / 2048.0f;
+        v = powf(v, 3.0f) * 6.0f;
+        gammaTable[i] = static_cast<unsigned short>(v * 6.0f * 256.0f);
+    }
+}
+
+Freenect::~Freenect()
+{
+    if (device != NULL)
+    {
+        freenect_stop_depth(device);
+        freenect_stop_rgb(device);
+    }
+
+    freenect_close_device(device);
+
+    freenect_shutdown(context);
+}
+
+bool
+Freenect::init(int userDeviceNumber)
+{
+    if (freenect_init(&context, NULL) < 0)
+    {
+        return false;
+    }
+
+    freenect_set_log_level(context, FREENECT_LOG_DEBUG);
+
+    if (freenect_num_devices(context) < 1)
+    {
+        return false;
+    }
+
+    if (freenect_open_device(context, &device, userDeviceNumber) < 0)
+    {
+        return false;
+    }
+
+    freenect_set_user(device, this);
+
+    memset(rgb, 0, FREENECT_RGB_SIZE);
+    memset(depth, 0, FREENECT_DEPTH_SIZE);
+
+    // set Kinect parameters
+    if (freenect_set_tilt_degs(device, tiltAngle) != 0)
+    {
+        return false;
+    }
+    if (freenect_set_led(device, LED_RED) != 0)
+    {
+        return false;
+    }
+    if (freenect_set_rgb_format(device, FREENECT_FORMAT_RGB) != 0)
+    {
+        return false;
+    }
+    if (freenect_set_depth_format(device, FREENECT_FORMAT_11_BIT) != 0)
+    {
+        return false;
+    }
+    freenect_set_rgb_callback(device, rgbCallback);
+    freenect_set_depth_callback(device, depthCallback);
+
+    if (freenect_start_rgb(device) != 0)
+    {
+        return false;
+    }
+    if (freenect_start_depth(device) != 0)
+    {
+        return false;
+    }
+
+    thread.reset(new FreenectThread(device));
+    thread->start();
+
+    return true;
+}
+
+bool
+Freenect::process(void)
+{
+    if (freenect_process_events(context) < 0)
+    {
+        return false;
+    }
+
+    freenect_get_raw_accel(device, &ax, &ay, &az);
+    freenect_get_mks_accel(device, &dx, &dy, &dz);
+
+    return true;
+}
+
+QSharedPointer<QByteArray>
+Freenect::getRgbData(void)
+{
+    QMutexLocker locker(&rgbMutex);
+    return QSharedPointer<QByteArray>(
+            new QByteArray(rgb, FREENECT_RGB_SIZE));
+}
+
+QSharedPointer<QByteArray>
+Freenect::getRawDepthData(void)
+{
+    QMutexLocker locker(&depthMutex);
+    return QSharedPointer<QByteArray>(
+            new QByteArray(depth, FREENECT_DEPTH_SIZE));
+}
+
+QSharedPointer<QByteArray>
+Freenect::getDistanceData(void)
+{
+    QMutexLocker locker(&distanceMutex);
+    return QSharedPointer<QByteArray>(
+            new QByteArray(reinterpret_cast<char *>(distance),
+                           FREENECT_FRAME_PIX * sizeof(float)));
+}
+
+QSharedPointer<QByteArray>
+Freenect::getColoredDepthData(void)
+{
+    QMutexLocker locker(&coloredDepthMutex);
+    return QSharedPointer<QByteArray>(
+            new QByteArray(coloredDepth, FREENECT_RGB_SIZE));
+}
+
+int
+Freenect::getTiltAngle(void) const
+{
+    return tiltAngle;
+}
+
+void
+Freenect::setTiltAngle(int angle)
+{
+    if (angle > 30)
+    {
+        angle = 30;
+    }
+    if (angle < -30)
+    {
+        angle = -30;
+    }
+
+    tiltAngle = angle;
+}
+
+Freenect::FreenectThread::FreenectThread(freenect_device* _device)
+{
+    device = _device;
+}
+
+void
+Freenect::FreenectThread::run(void)
+{
+    Freenect* freenect = static_cast<Freenect *>(freenect_get_user(device));
+    while (1)
+    {
+        freenect->process();
+    }
+}
+
+void
+Freenect::rgbCallback(freenect_device* device, freenect_pixel* rgb, uint32_t timestamp)
+{
+    Freenect* freenect = static_cast<Freenect *>(freenect_get_user(device));
+
+    QMutexLocker locker(&freenect->rgbMutex);
+    memcpy(freenect->rgb, rgb, FREENECT_RGB_SIZE);
+}
+
+void
+Freenect::depthCallback(freenect_device* device, freenect_depth* depth, uint32_t timestamp)
+{
+    Freenect* freenect = static_cast<Freenect *>(freenect_get_user(device));
+    freenect_depth* data = reinterpret_cast<freenect_depth *>(depth);
+
+    QMutexLocker depthLocker(&freenect->depthMutex);
+    memcpy(freenect->depth, data, FREENECT_DEPTH_SIZE);
+
+    QMutexLocker distanceLocker(&freenect->distanceMutex);
+    for (int i = 0; i < FREENECT_FRAME_PIX; ++i)
+    {
+        freenect->distance[i] =
+                100.f / (-0.00307f * static_cast<float>(data[i]) + 3.33f);
+    }
+
+    QMutexLocker coloredDepthLocker(&freenect->coloredDepthMutex);
+    unsigned short* src = reinterpret_cast<unsigned short *>(data);
+    unsigned char* dst = reinterpret_cast<unsigned char *>(freenect->coloredDepth);
+    for (int i = 0; i < FREENECT_FRAME_PIX; ++i)
+    {
+        unsigned short pval = freenect->gammaTable[src[i]];
+        unsigned short lb = pval & 0xFF;
+        switch (pval >> 8)
+        {
+        case 0:
+            dst[3 * i] = 255;
+            dst[3 * i + 1] = 255 - lb;
+            dst[3 * i + 2] = 255 - lb;
+            break;
+        case 1:
+            dst[3 * i] = 255;
+            dst[3 * i + 1] = lb;
+            dst[3 * i + 2] = 0;
+            break;
+        case 2:
+            dst[3 * i] = 255 - lb;
+            dst[3 * i + 1] = 255;
+            dst[3 * i + 2] = 0;
+            break;
+        case 3:
+            dst[3 * i] = 0;
+            dst[3 * i + 1] = 255;
+            dst[3 * i + 2] = lb;
+            break;
+        case 4:
+            dst[3 * i] = 0;
+            dst[3 * i + 1] = 255 - lb;
+            dst[3 * i + 2] = 255;
+            break;
+        case 5:
+            dst[3 * i] = 0;
+            dst[3 * i + 1] = 0;
+            dst[3 * i + 2] = 255 - lb;
+            break;
+        default:
+            dst[3 * i] = 0;
+            dst[3 * i + 1] = 0;
+            dst[3 * i + 2] = 0;
+            break;
+        }
+    }
+}

src/input/Freenect.h

+#ifndef FREENECT_H
+#define FREENECT_H
+
+#include <libfreenect.h>
+#include <QMutex>
+#include <QScopedPointer>
+#include <QSharedPointer>
+#include <QThread>
+
+class Freenect
+{
+public:
+    Freenect();
+    ~Freenect();
+
+    bool init(int userDeviceNumber = 0);
+    bool process(void);
+
+    QSharedPointer<QByteArray> getRgbData(void);
+    QSharedPointer<QByteArray> getRawDepthData(void);
+    QSharedPointer<QByteArray> getDistanceData(void);
+    QSharedPointer<QByteArray> getColoredDepthData(void);
+
+    int getTiltAngle(void) const;
+    void setTiltAngle(int angle);
+
+private:
+    static void rgbCallback(freenect_device* device, freenect_pixel* rgb, uint32_t timestamp);
+    static void depthCallback(freenect_device* device, freenect_depth* depth, uint32_t timestamp);
+
+    freenect_context* context;
+    freenect_device* device;
+
+    class FreenectThread : public QThread
+    {
+    public:
+        explicit FreenectThread(freenect_device* _device);
+
+    protected:
+        virtual void run(void);
+
+        freenect_device* device;
+    };
+    QScopedPointer<FreenectThread> thread;
+
+    // tilt angle of Kinect camera
+    int tiltAngle;
+
+    // rgbd data
+    char rgb[FREENECT_RGB_SIZE];
+    QMutex rgbMutex;
+
+    char depth[FREENECT_DEPTH_SIZE];
+    QMutex depthMutex;
+
+    float distance[FREENECT_FRAME_PIX];
+    QMutex distanceMutex;
+
+    char coloredDepth[FREENECT_RGB_SIZE];
+    QMutex coloredDepthMutex;
+
+    // accelerometer data
+    short ax, ay, az;
+    double dx, dy, dz;
+
+    // gamma map
+    unsigned short gammaTable[2048];
+};
+
+#endif // FREENECT_H

src/ui/MainWindow.cc

@@ -47,7 +47,7 @@ This file is part of the QGROUNDCONTROL project
 #include "GAudioOutput.h"
 
 #ifdef QGC_OSG_ENABLED
-#include "QMap3D.h"
+#include "Q3DWidgetFactory.h"
 #endif
 
 // FIXME Move
@@ -136,10 +136,13 @@ void MainWindow::buildWidgets()
     protocolWidget    = new XMLCommProtocolWidget(this);
     dataplotWidget    = new QGCDataPlot2D(this);
     #ifdef QGC_OSG_ENABLED
-    //_3DWidget         = Q3DWidgetFactory::get(QGC_MAP3D_OSGEARTH);
-    _3DWidget = new QMap3D(this);
+    _3DWidget         = Q3DWidgetFactory::get("PIXHAWK");
     #endif
 
+#ifdef QGC_OSGEARTH_ENABLED
+    _3DMapWidget = Q3DWidgetFactory::get("MAP3D");
+#endif
+
     // Dock widgets
     controlDockWidget = new QDockWidget(tr("Control"), this);
     controlDockWidget->setWidget( new UASControlWidget(this) );
@@ -232,6 +235,9 @@ void MainWindow::arrangeCenterStack()
     if (mapWidget) centerStack->addWidget(mapWidget);
     #ifdef QGC_OSG_ENABLED
     if (_3DWidget) centerStack->addWidget(_3DWidget);
+#endif
+    #ifdef QGC_OSGEARTH_ENABLED
+    if (_3DMapWidget) centerStack->addWidget(_3DMapWidget);
     #endif
     if (hudWidget) centerStack->addWidget(hudWidget);
     if (dataplotWidget) centerStack->addWidget(dataplotWidget);
@@ -373,6 +379,7 @@ void MainWindow::connectActions()
     connect(ui.actionEngineerView, SIGNAL(triggered()), this, SLOT(loadEngineerView()));
     connect(ui.actionOperatorView, SIGNAL(triggered()), this, SLOT(loadOperatorView()));
     connect(ui.action3DView, SIGNAL(triggered()), this, SLOT(load3DView()));
+    connect(ui.action3DMapView, SIGNAL(triggered()), this, SLOT(load3DMapView()));
     connect(ui.actionShow_full_view, SIGNAL(triggered()), this, SLOT(loadAllView()));
     connect(ui.actionShow_MAVLink_view, SIGNAL(triggered()), this, SLOT(loadMAVLinkView()));
     connect(ui.actionShow_data_analysis_view, SIGNAL(triggered()), this, SLOT(loadDataView()));
@@ -672,7 +679,7 @@ void MainWindow::loadPixhawkView()
     clearView();
     // Engineer view, used in EMAV2009
 
-        #ifdef QGC_OSG_ENABLED
+#ifdef QGC_OSG_ENABLED
     // 3D map
     if (_3DWidget)
     {
@@ -960,6 +967,60 @@ void MainWindow::loadGlobalOperatorView()
 
 }
 
+void MainWindow::load3DMapView()
+{
+        #ifdef QGC_OSGEARTH_ENABLED
+            clearView();
+
+            // 3D map
+            if (_3DMapWidget)
+            {
+                QStackedWidget *centerStack = dynamic_cast<QStackedWidget*>(centralWidget());
+                if (centerStack)
+                {
+                    //map3DWidget->setActive(true);
+                    centerStack->setCurrentWidget(_3DMapWidget);
+                }
+            }
+
+            // UAS CONTROL
+            if (controlDockWidget)
+            {
+                addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget);
+                controlDockWidget->show();
+            }
+
+            // UAS LIST
+            if (listDockWidget)
+            {
+                addDockWidget(Qt::BottomDockWidgetArea, listDockWidget);
+                listDockWidget->show();
+            }
+
+            // WAYPOINT LIST
+            if (waypointsDockWidget)
+            {
+                addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget);
+                waypointsDockWidget->show();
+            }
+
+            // HORIZONTAL SITUATION INDICATOR
+            if (hsiDockWidget)
+            {
+                HSIDisplay* hsi = dynamic_cast<HSIDisplay*>( hsiDockWidget->widget() );
+                if (hsi)
+                {
+                    hsi->start();
+                    addDockWidget(Qt::LeftDockWidgetArea, hsiDockWidget);
+                    hsiDockWidget->show();
+                }
+            }
+#endif
+            this->show();
+
+        }
+
+
 void MainWindow::load3DView()
 {
         #ifdef QGC_OSG_ENABLED

src/ui/MainWindow.h

@@ -63,10 +63,6 @@ This file is part of the QGROUNDCONTROL project
 #include "HSIDisplay.h"
 #include "QGCDataPlot2D.h"
 #include "QGCRemoteControlView.h"
-#ifdef QGC_OSG_ENABLED
-//#include "Q3DWidget.h"
-#include "QMap3D.h"
-#endif
 
 #include "LogCompressor.h"
 
@@ -117,6 +113,8 @@ public slots:
     void loadOperatorView();
     /** @brief Load 3D view */
     void load3DView();
+    /** @brief Load 3D map view */
+    void load3DMapView();
     /** @brief Load view with all widgets */
     void loadAllView();
     /** @brief Load MAVLink XML generator view */
@@ -167,7 +165,10 @@ protected:
     QPointer<XMLCommProtocolWidget> protocolWidget;
     QPointer<QGCDataPlot2D> dataplotWidget;
     #ifdef QGC_OSG_ENABLED
-    QPointer<QMap3D> _3DWidget;
+    QPointer<QWidget> _3DWidget;
+    #endif
+    #ifdef QGC_OSGEARTH_ENABLED
+    QPointer<QWidget> _3DMapWidget;
     #endif
     // Dock widgets
     QPointer<QDockWidget> controlDockWidget;

src/ui/MainWindow.ui

@@ -75,6 +75,7 @@
     <addaction name="actionEngineerView"/>
     <addaction name="actionPilotView"/>
     <addaction name="actionOperatorView"/>
+    <addaction name="action3DMapView"/>
     <addaction name="action3DView"/>
     <addaction name="actionGlobalOperatorView"/>
     <addaction name="separator"/>
@@ -234,7 +235,7 @@
      <normaloff>:/images/categories/preferences-system.svg</normaloff>:/images/categories/preferences-system.svg</iconset>
    </property>
    <property name="text">
-    <string>Show 3D view</string>
+    <string>Show 3D local view</string>
    </property>
    <property name="toolTip">
     <string>Show 3D view</string>
@@ -318,6 +319,15 @@
     <string>Show Global operator view</string>
    </property>
   </action>
+  <action name="action3DMapView">
+   <property name="icon">
+    <iconset resource="../../mavground.qrc">
+     <normaloff>:/images/categories/applications-internet.svg</normaloff>:/images/categories/applications-internet.svg</iconset>
+   </property>
+   <property name="text">
+    <string>Show 3D Globe view</string>
+   </property>
+  </action>
  </widget>
  <layoutdefault spacing="6" margin="11"/>
  <resources>

src/ui/map3D/GCManipulator.cc

@@ -254,7 +254,7 @@ GCManipulator::calcMovement()
     if (buttonMask == GUIEventAdapter::LEFT_MOUSE_BUTTON)
     {
         // rotate camera
-        osg::Vec3 axis;
+        osg::Vec3d axis;
         float angle;
 
         float px0 = _ga_t0->getXnormalized();

src/ui/map3D/ImageWindowGeode.cc

+///*=====================================================================
+//
+//QGroundControl Open Source Ground Control Station
+//
+//(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+//
+//This file is part of the QGROUNDCONTROL project
+//
+//    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+//    it under the terms of the GNU General Public License as published by
+//    the Free Software Foundation, either version 3 of the License, or
+//    (at your option) any later version.
+//
+//    QGROUNDCONTROL is distributed in the hope that it will be useful,
+//    but WITHOUT ANY WARRANTY; without even the implied warranty of
+//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//    GNU General Public License for more details.
+//
+//    You should have received a copy of the GNU General Public License
+//    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+//
+//======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of the class ImageWindowGeode.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
+#include "ImageWindowGeode.h"
+
+ImageWindowGeode::ImageWindowGeode(const QString& caption,
+                                   const osg::Vec4& backgroundColor,
+                                   osg::ref_ptr<osg::Image>& image)
+    : border(5)
+{
+    // image
+    osg::ref_ptr<osg::Geometry> imageGeometry = new osg::Geometry;
+    imageVertices = new osg::Vec3Array(4);
+
+    osg::ref_ptr<osg::Vec2Array> textureCoords = new osg::Vec2Array;
+    textureCoords->push_back(osg::Vec2(0.0f, 1.0f));
+    textureCoords->push_back(osg::Vec2(1.0f, 1.0f));
+    textureCoords->push_back(osg::Vec2(1.0f, 0.0f));
+    textureCoords->push_back(osg::Vec2(0.0f, 0.0f));
+
+    osg::ref_ptr<osg::Vec4Array> imageColors(new osg::Vec4Array);
+    imageColors->push_back(osg::Vec4(1.0f, 1.0f, 1.0f, 1.0f));
+    imageGeometry->setColorArray(imageColors);
+    imageGeometry->setColorBinding(osg::Geometry::BIND_OVERALL);
+
+    imageGeometry->setVertexArray(imageVertices);
+    imageGeometry->setTexCoordArray(0, textureCoords);
+
+    imageGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON,
+                                                       0, imageVertices->size()));
+
+    osg::ref_ptr<osg::Texture2D> texture = new osg::Texture2D;
+    texture->setDataVariance(osg::Object::DYNAMIC);
+    texture->setImage(image);
+    texture->setResizeNonPowerOfTwoHint(false);
+
+    imageGeometry->getOrCreateStateSet()->
+            setTextureAttributeAndModes(0, texture, osg::StateAttribute::ON);
+    imageGeometry->setUseDisplayList(false);
+
+    // background
+    osg::ref_ptr<osg::Geometry> backgroundGeometry = new osg::Geometry;
+    backgroundVertices = new osg::Vec3Array(4);
+    backgroundGeometry->setVertexArray(backgroundVertices);
+    backgroundGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON,
+                                                            0, backgroundVertices->size()));
+    osg::ref_ptr<osg::Vec4Array> backgroundColors(new osg::Vec4Array);
+    backgroundColors->push_back(backgroundColor);
+    backgroundGeometry->setColorArray(backgroundColors);
+    backgroundGeometry->setColorBinding(osg::Geometry::BIND_OVERALL);
+    backgroundGeometry->setUseDisplayList(false);
+
+    // caption
+    text = new osgText::Text;
+    text->setText(caption.toStdString().c_str());
+    text->setCharacterSize(11);
+    text->setFont("images/Vera.ttf");
+    text->setAxisAlignment(osgText::Text::SCREEN);
+    text->setColor(osg::Vec4(1.0f, 1.0f, 1.0f, 1.0f));
+
+    addDrawable(imageGeometry);
+    addDrawable(backgroundGeometry);
+    addDrawable(text);
+
+    setAttributes(0, 0, 0, 0);
+}
+
+void
+ImageWindowGeode::setAttributes(int x, int y, int width, int height)
+{
+    int imageWidth = width - border * 2;
+    int imageHeight = height - border * 2 - 15;
+    int imageXPosition = x + border;
+    int imageYPosition = y + border;
+
+    imageVertices->at(0) = osg::Vec3(imageXPosition, imageYPosition, 0);
+    imageVertices->at(1) = osg::Vec3(imageXPosition + imageWidth, imageYPosition, 0);
+    imageVertices->at(2) = osg::Vec3(imageXPosition + imageWidth, imageYPosition + imageHeight, 0);
+    imageVertices->at(3) = osg::Vec3(imageXPosition, imageYPosition + imageHeight, 0);
+
+    text->setPosition(osg::Vec3(imageXPosition, imageYPosition + imageHeight + 5, 0));
+
+    backgroundVertices->at(0) = osg::Vec3(x, y, -1);
+    backgroundVertices->at(1) = osg::Vec3(x + width, y, -1);
+    backgroundVertices->at(2) = osg::Vec3(x + width, y + height, -1);
+    backgroundVertices->at(3) = osg::Vec3(x, y + height, -1);
+}

src/ui/map3D/ImageWindowGeode.h

+///*=====================================================================
+//
+//QGroundControl Open Source Ground Control Station
+//
+//(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+//
+//This file is part of the QGROUNDCONTROL project
+//
+//    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+//    it under the terms of the GNU General Public License as published by
+//    the Free Software Foundation, either version 3 of the License, or
+//    (at your option) any later version.
+//
+//    QGROUNDCONTROL is distributed in the hope that it will be useful,
+//    but WITHOUT ANY WARRANTY; without even the implied warranty of
+//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//    GNU General Public License for more details.
+//
+//    You should have received a copy of the GNU General Public License
+//    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+//
+//======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of the class ImageWindowGeode.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
+#ifndef IMAGEWINDOWGEODE_H
+#define IMAGEWINDOWGEODE_H
+
+#include <osg/Geode>
+#include <osg/Geometry>
+#include <osgText/Text>
+#include <QString>
+
+class ImageWindowGeode : public osg::Geode
+{
+public:
+    ImageWindowGeode(const QString& caption, const osg::Vec4& backgroundColor,
+                     osg::ref_ptr<osg::Image>& image);
+
+    void setAttributes(int x, int y, int width, int height);
+
+private:
+    int border;
+
+    osg::ref_ptr<osg::Vec3Array> imageVertices;
+    osg::ref_ptr<osg::Vec3Array> backgroundVertices;
+    osg::ref_ptr<osgText::Text> text;
+};
+
+#endif // IMAGEWINDOWGEODE_H

src/ui/map3D/Imagery.cc

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of the class Imagery.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
+#include "Imagery.h"
+
+#include <cmath>
+#include <iomanip>
+#include <sstream>
+
+const double WGS84_A = 6378137.0;
+const double WGS84_ECCSQ = 0.00669437999013;
+
+const int MAX_ZOOM_LEVEL = 20;
+
+Imagery::Imagery()
+    : textureCache(new TextureCache(1000))
+{
+
+}
+
+void
+Imagery::setImageryType(ImageryType type)
+{
+    currentImageryType = type;
+}
+
+void
+Imagery::setOffset(double xOffset, double yOffset)
+{
+    this->xOffset = xOffset;
+    this->yOffset = yOffset;
+}
+
+void
+Imagery::prefetch2D(double windowWidth, double windowHeight,
+                    double zoom, double xOrigin, double yOrigin,
+                    double viewXOffset, double viewYOffset,
+                    const QString& utmZone)
+{
+    double tileResolution;
+    if (currentImageryType == GOOGLE_SATELLITE ||
+        currentImageryType == GOOGLE_MAP)
+    {
+        tileResolution = 1.0;
+        while (tileResolution * 3.0 / 2.0 < 1.0 / zoom)
+        {
+            tileResolution *= 2.0;
+        }
+        if (tileResolution > 512.0)
+        {
+            tileResolution = 512.0;
+        }
+    }
+    else if (currentImageryType == SWISSTOPO_SATELLITE)
+    {
+        tileResolution = 0.25;
+    }
+
+    int minTileX, minTileY, maxTileX, maxTileY;
+    int zoomLevel;
+
+    tileBounds(tileResolution,
+               xOrigin + viewXOffset - windowWidth / 2.0 / zoom,
+               yOrigin + viewYOffset - windowHeight / 2.0 / zoom,
+               xOrigin + viewXOffset + windowWidth / 2.0 / zoom,
+               yOrigin + viewYOffset + windowHeight / 2.0 / zoom, utmZone,
+               minTileX, minTileY, maxTileX, maxTileY, zoomLevel);
+
+    for (int r = minTileY; r <= maxTileY; ++r)
+    {
+        for (int c = minTileX; c <= maxTileX; ++c)
+        {
+            QString url = getTileLocation(c, r, zoomLevel, tileResolution);
+
+            TexturePtr t = textureCache->get(url);
+        }
+    }
+}
+
+void
+Imagery::draw2D(double windowWidth, double windowHeight,
+                double zoom, double xOrigin, double yOrigin,
+                double viewXOffset, double viewYOffset,
+                const QString& utmZone)
+{
+    double tileResolution;
+    if (currentImageryType == GOOGLE_SATELLITE ||
+        currentImageryType == GOOGLE_MAP)
+    {
+        tileResolution = 1.0;
+        while (tileResolution * 3.0 / 2.0 < 1.0 / zoom)
+        {
+            tileResolution *= 2.0;
+        }
+        if (tileResolution > 512.0)
+        {
+            tileResolution = 512.0;
+        }
+    }
+    else if (currentImageryType == SWISSTOPO_SATELLITE)
+    {
+        tileResolution = 0.25;
+    }
+
+    int minTileX, minTileY, maxTileX, maxTileY;
+    int zoomLevel;
+
+    tileBounds(tileResolution,
+               xOrigin + viewXOffset - windowWidth / 2.0 / zoom * 1.5,
+               yOrigin + viewYOffset - windowHeight / 2.0 / zoom * 1.5,
+               xOrigin + viewXOffset + windowWidth / 2.0 / zoom * 1.5,
+               yOrigin + viewYOffset + windowHeight / 2.0 / zoom * 1.5, utmZone,
+               minTileX, minTileY, maxTileX, maxTileY, zoomLevel);
+
+    for (int r = minTileY; r <= maxTileY; ++r)
+    {
+        for (int c = minTileX; c <= maxTileX; ++c)
+        {
+            QString tileURL = getTileLocation(c, r, zoomLevel, tileResolution);
+
+            double x1, y1, x2, y2, x3, y3, x4, y4;
+            imageBounds(c, r, tileResolution, x1, y1, x2, y2, x3, y3, x4, y4);
+
+            TexturePtr t = textureCache->get(tileURL);
+            if (!t.isNull())
+            {
+                t->draw(x1 - xOrigin, y1 - yOrigin,
+                        x2 - xOrigin, y2 - yOrigin,
+                        x3 - xOrigin, y3 - yOrigin,
+                        x4 - xOrigin, y4 - yOrigin, true);
+            }
+        }
+    }
+}
+
+void
+Imagery::prefetch3D(double radius, double tileResolution,
+                    double xOrigin, double yOrigin,
+                    double viewXOffset, double viewYOffset,
+                    const QString& utmZone, bool useHeightModel)
+{
+    int minTileX, minTileY, maxTileX, maxTileY;
+    int zoomLevel;
+
+    tileBounds(tileResolution,
+               xOrigin + viewXOffset - radius,
+               yOrigin + viewYOffset - radius,
+               xOrigin + viewXOffset + radius,
+               yOrigin + viewYOffset + radius, utmZone,
+               minTileX, minTileY, maxTileX, maxTileY, zoomLevel);
+
+    for (int r = minTileY; r <= maxTileY; ++r)
+    {
+        for (int c = minTileX; c <= maxTileX; ++c)
+        {
+            QString url = getTileLocation(c, r, zoomLevel, tileResolution);
+
+            TexturePtr t = textureCache->get(url, useHeightModel);
+        }
+    }
+}
+
+void
+Imagery::draw3D(double radius, double tileResolution,
+                double xOrigin, double yOrigin,
+                double viewXOffset, double viewYOffset,
+                const QString& utmZone, bool useHeightModel)
+{
+    int minTileX, minTileY, maxTileX, maxTileY;
+    int zoomLevel;
+
+    tileBounds(tileResolution,
+               xOrigin + viewXOffset - radius,
+               yOrigin + viewYOffset - radius,
+               xOrigin + viewXOffset + radius,
+               yOrigin + viewYOffset + radius, utmZone,
+               minTileX, minTileY, maxTileX, maxTileY, zoomLevel);
+
+    for (int r = minTileY; r <= maxTileY; ++r)
+    {
+        for (int c = minTileX; c <= maxTileX; ++c)
+        {
+            QString tileURL = getTileLocation(c, r, zoomLevel, tileResolution);
+
+            double x1, y1, x2, y2, x3, y3, x4, y4;
+            imageBounds(c, r, tileResolution, x1, y1, x2, y2, x3, y3, x4, y4);
+
+            TexturePtr t = textureCache->get(tileURL, useHeightModel);
+
+            if (!t.isNull())
+            {
+                t->draw(x1 - xOrigin, y1 - yOrigin,
+                        x2 - xOrigin, y2 - yOrigin,
+                        x3 - xOrigin, y3 - yOrigin,
+                        x4 - xOrigin, y4 - yOrigin, true);
+            }
+        }
+    }
+}
+
+bool
+Imagery::update(void)
+{
+    textureCache->sync();
+
+    return true;
+}
+
+void
+Imagery::imageBounds(int tileX, int tileY, double tileResolution,
+                     double& x1, double& y1, double& x2, double& y2,
+                     double& x3, double& y3, double& x4, double& y4) const
+{
+    if (currentImageryType == GOOGLE_MAP ||
+        currentImageryType == GOOGLE_SATELLITE)
+    {
+        int zoomLevel = MAX_ZOOM_LEVEL - static_cast<int>(rint(log2(tileResolution)));
+        int numTiles = static_cast<int>(exp2(static_cast<double>(zoomLevel)));
+
+        double lon1 = tileXToLongitude(tileX, numTiles);
+        double lon2 = tileXToLongitude(tileX + 1, numTiles);
+
+        double lat1 = tileYToLatitude(tileY, numTiles);
+        double lat2 = tileYToLatitude(tileY + 1, numTiles);
+
+        QString utmZone;
+        LLtoUTM(lat1, lon1, x1, y1, utmZone);
+        LLtoUTM(lat1, lon2, x2, y2, utmZone);
+        LLtoUTM(lat2, lon2, x3, y3, utmZone);
+        LLtoUTM(lat2, lon1, x4, y4, utmZone);
+    }
+    else if (currentImageryType == SWISSTOPO_SATELLITE ||
+             currentImageryType == SWISSTOPO_SATELLITE_3D)
+    {
+        double utmMultiplier = tileResolution * 200.0;
+        double minX = tileX * utmMultiplier;
+        double maxX = minX + utmMultiplier;
+        double minY = tileY * utmMultiplier;
+        double maxY = minY + utmMultiplier;
+
+        x1 = maxX; y1 = minY;
+        x2 = maxX; y2 = maxY;
+        x3 = minX; y3 = maxY;
+        x4 = minX; y4 = minY;
+    }
+}
+
+void
+Imagery::tileBounds(double tileResolution,
+                    double minUtmX, double minUtmY,
+                    double maxUtmX, double maxUtmY, const QString& utmZone,
+                    int& minTileX, int& minTileY,
+                    int& maxTileX, int& maxTileY,
+                    int& zoomLevel) const
+{
+    double centerUtmX = (maxUtmX - minUtmX) / 2.0 + minUtmX;
+    double centerUtmY = (maxUtmY - minUtmY) / 2.0 + minUtmY;
+    int centerTileX, centerTileY;
+
+    if (currentImageryType == GOOGLE_MAP ||
+        currentImageryType == GOOGLE_SATELLITE)
+    {
+        UTMtoTile(minUtmX, minUtmY, utmZone, tileResolution,
+                  minTileX, maxTileY, zoomLevel);
+        UTMtoTile(centerUtmX, centerUtmY, utmZone, tileResolution,
+                  centerTileX, centerTileY, zoomLevel);
+        UTMtoTile(maxUtmX, maxUtmY, utmZone, tileResolution,
+                  maxTileX, minTileY, zoomLevel);
+    }
+    else if (currentImageryType == SWISSTOPO_SATELLITE ||
+             currentImageryType == SWISSTOPO_SATELLITE_3D)
+    {
+        double utmMultiplier = tileResolution * 200;
+
+        minTileX = static_cast<int>(rint(minUtmX / utmMultiplier));
+        minTileY = static_cast<int>(rint(minUtmY / utmMultiplier));
+        centerTileX = static_cast<int>(rint(centerUtmX / utmMultiplier));
+        centerTileY = static_cast<int>(rint(centerUtmY / utmMultiplier));
+        maxTileX = static_cast<int>(rint(maxUtmX / utmMultiplier));
+        maxTileY = static_cast<int>(rint(maxUtmY / utmMultiplier));
+    }
+
+    if (maxTileX - minTileX + 1 > 14)
+    {
+        minTileX = centerTileX - 7;
+        maxTileX = centerTileX + 6;
+    }
+    if (maxTileY - minTileY + 1 > 14)
+    {
+        minTileY = centerTileY - 7;
+        maxTileY = centerTileY + 6;
+    }
+}
+
+double
+Imagery::tileXToLongitude(int tileX, int numTiles) const
+{
+    return 360.0 * (static_cast<double>(tileX)
+                    / static_cast<double>(numTiles)) - 180.0;
+}
+
+double
+Imagery::tileYToLatitude(int tileY, int numTiles) const
+{
+    double unnormalizedRad =
+            (static_cast<double>(tileY) / static_cast<double>(numTiles))
+            * 2.0 * M_PI - M_PI;
+    double rad = 2.0 * atan(exp(unnormalizedRad)) - M_PI / 2.0;
+    return -rad * 180.0 / M_PI;
+}
+
+int
+Imagery::longitudeToTileX(double longitude, int numTiles) const
+{
+    return static_cast<int>((longitude / 180.0 + 1.0) / 2.0 * numTiles);
+}
+
+int
+Imagery::latitudeToTileY(double latitude, int numTiles) const
+{
+    double rad = latitude * M_PI / 180.0;
+    double normalizedRad = -log(tan(rad) + 1.0 / cos(rad));
+    return static_cast<int>((normalizedRad + M_PI)
+                                / (2.0 * M_PI) * numTiles);
+}
+
+void
+Imagery::UTMtoTile(double northing, double easting, const QString& utmZone,
+                   double tileResolution, int& tileX, int& tileY,
+                   int& zoomLevel) const
+{
+    double latitude, longitude;
+
+    UTMtoLL(northing, easting, utmZone, latitude, longitude);
+
+    zoomLevel = MAX_ZOOM_LEVEL - static_cast<int>(rint(log2(tileResolution)));
+    int numTiles = static_cast<int>(exp2(static_cast<double>(zoomLevel)));
+
+    tileX = longitudeToTileX(longitude, numTiles);
+    tileY = latitudeToTileY(latitude, numTiles);
+}
+
+QChar
+Imagery::UTMLetterDesignator(double latitude) const
+{
+    // This routine determines the correct UTM letter designator for the given latitude
+    // returns 'Z' if latitude is outside the UTM limits of 84N to 80S
+    // Written by Chuck Gantz- chuck.gantz@globalstar.com
+    char letterDesignator;
+
+    if ((84.0 >= latitude) && (latitude >= 72.0)) letterDesignator = 'X';
+    else if ((72.0 > latitude) && (latitude >= 64.0)) letterDesignator = 'W';
+    else if ((64.0 > latitude) && (latitude >= 56.0)) letterDesignator = 'V';
+    else if ((56.0 > latitude) && (latitude >= 48.0)) letterDesignator = 'U';
+    else if ((48.0 > latitude) && (latitude >= 40.0)) letterDesignator = 'T';
+    else if ((40.0 > latitude) && (latitude >= 32.0)) letterDesignator = 'S';
+    else if ((32.0 > latitude) && (latitude >= 24.0)) letterDesignator = 'R';
+    else if ((24.0 > latitude) && (latitude >= 16.0)) letterDesignator = 'Q';
+    else if ((16.0 > latitude) && (latitude >= 8.0)) letterDesignator = 'P';
+    else if (( 8.0 > latitude) && (latitude >= 0.0)) letterDesignator = 'N';
+    else if (( 0.0 > latitude) && (latitude >= -8.0)) letterDesignator = 'M';
+    else if ((-8.0 > latitude) && (latitude >= -16.0)) letterDesignator = 'L';
+    else if ((-16.0 > latitude) && (latitude >= -24.0)) letterDesignator = 'K';
+    else if ((-24.0 > latitude) && (latitude >= -32.0)) letterDesignator = 'J';
+    else if ((-32.0 > latitude) && (latitude >= -40.0)) letterDesignator = 'H';
+    else if ((-40.0 > latitude) && (latitude >= -48.0)) letterDesignator = 'G';
+    else if ((-48.0 > latitude) && (latitude >= -56.0)) letterDesignator = 'F';
+    else if ((-56.0 > latitude) && (latitude >= -64.0)) letterDesignator = 'E';
+    else if ((-64.0 > latitude) && (latitude >= -72.0)) letterDesignator = 'D';
+    else if ((-72.0 > latitude) && (latitude >= -80.0)) letterDesignator = 'C';
+    else letterDesignator = 'Z'; //This is here as an error flag to show that the Latitude is outside the UTM limits
+
+    return letterDesignator;
+}
+
+void
+Imagery::LLtoUTM(double latitude, double longitude,
+                 double& utmNorthing, double& utmEasting,
+                 QString& utmZone) const
+{
+    // converts lat/long to UTM coords.  Equations from USGS Bulletin 1532
+    // East Longitudes are positive, West longitudes are negative.
+    // North latitudes are positive, South latitudes are negative
+    // Lat and Long are in decimal degrees
+    // Written by Chuck Gantz- chuck.gantz@globalstar.com
+
+    double k0 = 0.9996;
+
+    double LongOrigin;
+    double eccPrimeSquared;
+    double N, T, C, A, M;
+
+    double LatRad = latitude * M_PI / 180.0;
+    double LongRad = longitude * M_PI / 180.0;
+    double LongOriginRad;
+
+    int ZoneNumber = static_cast<int>((longitude + 180.0) / 6.0) + 1;
+
+    if (latitude >= 56.0 && latitude < 64.0 &&
+        longitude >= 3.0 && longitude < 12.0)
+    {
+        ZoneNumber = 32;
+    }
+
+    // Special zones for Svalbard
+    if (latitude >= 72.0 && latitude < 84.0)
+    {
+        if (     longitude >= 0.0  && longitude <  9.0) ZoneNumber = 31;
+        else if (longitude >= 9.0  && longitude < 21.0) ZoneNumber = 33;
+        else if (longitude >= 21.0 && longitude < 33.0) ZoneNumber = 35;
+        else if (longitude >= 33.0 && longitude < 42.0) ZoneNumber = 37;
+     }
+    LongOrigin = static_cast<double>((ZoneNumber - 1) * 6 - 180 + 3);  //+3 puts origin in middle of zone
+    LongOriginRad = LongOrigin * M_PI / 180.0;
+
+    // compute the UTM Zone from the latitude and longitude
+    utmZone = QString("%1%2").arg(ZoneNumber).arg(UTMLetterDesignator(latitude));
+
+    eccPrimeSquared = WGS84_ECCSQ / (1.0 - WGS84_ECCSQ);
+
+    N = WGS84_A / sqrt(1.0f - WGS84_ECCSQ * sin(LatRad) * sin(LatRad));
+    T = tan(LatRad) * tan(LatRad);
+    C = eccPrimeSquared * cos(LatRad) * cos(LatRad);
+    A = cos(LatRad) * (LongRad - LongOriginRad);
+
+    M = WGS84_A * ((1.0 - WGS84_ECCSQ / 4.0
+                    - 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 64.0
+                    - 5.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 256.0)
+                   * LatRad
+                   - (3.0 * WGS84_ECCSQ / 8.0
+                      + 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 32.0
+                      + 45.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 1024.0)
+                   * sin(2.0 * LatRad)
+                   + (15.0 * WGS84_ECCSQ * WGS84_ECCSQ / 256.0
+                      + 45.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 1024.0)
+                   * sin(4.0 * LatRad)
+                   - (35.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 3072.0)
+                   * sin(6.0 * LatRad));
+
+    utmEasting = k0 * N * (A + (1.0 - T + C) * A * A * A / 6.0
+                           + (5.0 - 18.0 * T + T * T + 72.0 * C
+                              - 58.0 * eccPrimeSquared)
+                           * A * A * A * A * A / 120.0)
+                 + 500000.0;
+
+    utmNorthing = k0 * (M + N * tan(LatRad) *
+                        (A * A / 2.0 +
+                         (5.0 - T + 9.0 * C + 4.0 * C * C) * A * A * A * A / 24.0
+                         + (61.0 - 58.0 * T + T * T + 600.0 * C
+                            - 330.0 * eccPrimeSquared)
+                         * A * A * A * A * A * A / 720.0));
+    if (latitude < 0.0)
+    {
+        utmNorthing += 10000000.0; //10000000 meter offset for southern hemisphere
+    }
+}
+
+void
+Imagery::UTMtoLL(double utmNorthing, double utmEasting, const QString& utmZone,
+                 double& latitude, double& longitude) const
+{
+    // converts UTM coords to lat/long.  Equations from USGS Bulletin 1532
+    // East Longitudes are positive, West longitudes are negative.
+    // North latitudes are positive, South latitudes are negative
+    // Lat and Long are in decimal degrees.
+    // Written by Chuck Gantz- chuck.gantz@globalstar.com
+
+    double k0 = 0.9996;
+    double eccPrimeSquared;
+    double e1 = (1.0 - sqrt(1.0 - WGS84_ECCSQ)) / (1.0 + sqrt(1.0 - WGS84_ECCSQ));
+    double N1, T1, C1, R1, D, M;
+    double LongOrigin;
+    double mu, phi1, phi1Rad;
+    double x, y;
+    int ZoneNumber;
+    char ZoneLetter;
+    bool NorthernHemisphere;
+
+    x = utmEasting - 500000.0; //remove 500,000 meter offset for longitude
+    y = utmNorthing;
+
+    std::istringstream iss(utmZone.toStdString());
+    iss >> ZoneNumber >> ZoneLetter;
+    if ((ZoneLetter - 'N') >= 0)
+    {
+        NorthernHemisphere = true;//point is in northern hemisphere
+    }
+    else
+    {
+        NorthernHemisphere = false;//point is in southern hemisphere
+        y -= 10000000.0;//remove 10,000,000 meter offset used for southern hemisphere
+    }
+
+    LongOrigin = (ZoneNumber - 1.0) * 6.0 - 180.0 + 3.0;  //+3 puts origin in middle of zone
+
+    eccPrimeSquared = WGS84_ECCSQ / (1.0 - WGS84_ECCSQ);
+
+    M = y / k0;
+    mu = M / (WGS84_A * (1.0 - WGS84_ECCSQ / 4.0
+                         - 3.0 * WGS84_ECCSQ * WGS84_ECCSQ / 64.0
+                         - 5.0 * WGS84_ECCSQ * WGS84_ECCSQ * WGS84_ECCSQ / 256.0));
+
+    phi1Rad = mu + (3.0 * e1 / 2.0 - 27.0 * e1 * e1 * e1 / 32.0) * sin(2.0 * mu)
+              + (21.0 * e1 * e1 / 16.0 - 55.0 * e1 * e1 * e1 * e1 / 32.0)
+              * sin(4.0 * mu)
+              + (151.0 * e1 * e1 * e1 / 96.0) * sin(6.0 * mu);
+    phi1 = phi1Rad / M_PI * 180.0;
+
+    N1 = WGS84_A / sqrt(1.0 - WGS84_ECCSQ * sin(phi1Rad) * sin(phi1Rad));
+    T1 = tan(phi1Rad) * tan(phi1Rad);
+    C1 = eccPrimeSquared * cos(phi1Rad) * cos(phi1Rad);
+    R1 = WGS84_A * (1.0 - WGS84_ECCSQ) /
+         pow(1.0 - WGS84_ECCSQ * sin(phi1Rad) * sin(phi1Rad), 1.5);
+    D = x / (N1 * k0);
+
+    latitude = phi1Rad - (N1 * tan(phi1Rad) / R1)
+               * (D * D / 2.0 - (5.0 + 3.0 * T1 + 10.0 * C1 - 4.0 * C1 * C1
+                                 - 9.0 * eccPrimeSquared) * D * D * D * D / 24.0
+                  + (61.0 + 90.0 * T1 + 298.0 * C1 + 45.0 * T1 * T1
+                     - 252.0 * eccPrimeSquared - 3.0 * C1 * C1)
+                  * D * D * D * D * D * D / 720.0);
+    latitude *= 180.0 / M_PI;
+
+    longitude = (D - (1.0 + 2.0 * T1 + C1) * D * D * D / 6.0
+                 + (5.0 - 2.0 * C1 + 28.0 * T1 - 3.0 * C1 * C1
+                    + 8.0 * eccPrimeSquared + 24.0 * T1 * T1)
+                 * D * D * D * D * D / 120.0) / cos(phi1Rad);
+    longitude = LongOrigin + longitude / M_PI * 180.0;
+}
+
+QString
+Imagery::getTileLocation(int tileX, int tileY, int zoomLevel,
+                         double tileResolution) const
+{
+    std::ostringstream oss;
+
+    switch (currentImageryType)
+    {
+    case GOOGLE_MAP:
+        oss << "http://mt0.google.com/vt/lyrs=m@120&x=" << tileX
+            << "&y=" << tileY << "&z=" << zoomLevel;
+        break;
+    case GOOGLE_SATELLITE:
+        oss << "http://khm.google.com/vt/lbw/lyrs=y&x=" << tileX
+            << "&y=" << tileY << "&z=" << zoomLevel;
+        break;
+    case SWISSTOPO_SATELLITE: case SWISSTOPO_SATELLITE_3D:
+        oss << "../map/eth_zurich_swissimage_025/200/color/" << tileY
+            << "/tile-";
+        if (tileResolution < 1.0)
+        {
+            oss << std::fixed << std::setprecision(2) << tileResolution;
+        }
+        else
+        {
+            oss << static_cast<int>(rint(tileResolution));
+        }
+        oss << "-" << tileY << "-" << tileX << ".jpg";
+    default:
+        {};
+    }
+
+    QString url(oss.str().c_str());
+
+    return url;
+}

src/ui/map3D/Imagery.h

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of the class Imagery.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
+#ifndef IMAGERY_H
+#define IMAGERY_H
+
+#include <QScopedPointer>
+#include <QString>
+
+#include "TextureCache.h"
+
+class Imagery
+{
+public:
+    enum ImageryType
+    {
+        GOOGLE_MAP = 0,
+        GOOGLE_SATELLITE = 1,
+        SWISSTOPO_SATELLITE = 2,
+        SWISSTOPO_SATELLITE_3D = 3
+    };
+
+    Imagery();
+
+    void setImageryType(ImageryType type);
+    void setOffset(double xOffset, double yOffset);
+
+    void prefetch2D(double windowWidth, double windowHeight,
+                    double zoom, double xOrigin, double yOrigin,
+                    double viewXOffset, double viewYOffset,
+                    const QString& utmZone);
+    void draw2D(double windowWidth, double windowHeight,
+                double zoom, double xOrigin, double yOrigin,
+                double viewXOffset, double viewYOffset,
+                const QString& utmZone);
+
+    void prefetch3D(double radius, double tileResolution,
+                    double xOrigin, double yOrigin,
+                    double viewXOffset, double viewYOffset,
+                    const QString& utmZone, bool useHeightModel);
+    void draw3D(double radius, double tileResolution,
+                double xOrigin, double yOrigin,
+                double viewXOffset, double viewYOffset,
+                const QString& utmZone, bool useHeightModel);
+
+    bool update(void);
+
+private:
+    void imageBounds(int tileX, int tileY, double tileResolution,
+                     double& x1, double& y1, double& x2, double& y2,
+                     double& x3, double& y3, double& x4, double& y4) const;
+    void tileBounds(double tileResolution,
+                    double minUtmX, double minUtmY,
+                    double maxUtmX, double maxUtmY, const QString& utmZone,
+                    int& minTileX, int& minTileY,
+                    int& maxTileX, int& maxTileY,
+                    int& zoomLevel) const;
+
+    double tileXToLongitude(int tileX, int numTiles) const;
+    double tileYToLatitude(int tileY, int numTiles) const;
+    int longitudeToTileX(double longitude, int numTiles) const;
+    int latitudeToTileY(double latitude, int numTiles) const;
+
+    void UTMtoTile(double northing, double easting, const QString& utmZone,
+                   double tileResolution, int& tileX, int& tileY,
+                   int& zoomLevel) const;
+    QChar UTMLetterDesignator(double latitude) const;
+
+    void LLtoUTM(double latitude, double longitude,
+                 double& utmNorthing, double& utmEasting,
+                 QString& utmZone) const;
+    void UTMtoLL(double utmNorthing, double utmEasting, const QString& utmZone,
+                 double& latitude, double& longitude) const;
+
+    QString getTileLocation(int tileX, int tileY, int zoomLevel,
+                            double tileResolution) const;
+
+    QScopedPointer<TextureCache> textureCache;
+
+    ImageryType currentImageryType;
+
+    double xOffset;
+    double yOffset;
+};
+
+#endif // IMAGERY_H

src/ui/map3D/Pixhawk3DWidget.cc

@@ -34,6 +34,8 @@
 #include <sstream>
 
 #include <osg/Geode>
+#include <osg/Image>
+#include <osgDB/ReadFile>
 #include <osg/LineWidth>
 #include <osg/ShapeDrawable>
 
@@ -49,6 +51,8 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
      , displayTrail(false)
      , displayTarget(false)
      , displayWaypoints(true)
+     , displayRGBD2D(false)
+     , displayRGBD3D(false)
      , followCamera(true)
      , lastRobotX(0.0f)
      , lastRobotY(0.0f)
@@ -58,7 +62,8 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
     init(15.0f);
 
     // generate Pixhawk Cheetah model
-    egocentricMap->addChild(PixhawkCheetahGeode::instance());
+    vehicleModel = PixhawkCheetahGeode::instance();
+    egocentricMap->addChild(vehicleModel);
 
     // generate grid model
     gridNode = createGrid();
@@ -68,9 +73,11 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
     trailNode = createTrail();
     rollingMap->addChild(trailNode);
 
+#ifdef QGC_OSGEARTH_ENABLED
     // generate map model
     mapNode = createMap();
     root->addChild(mapNode);
+#endif
 
     // generate target model
     allocentricMap->addChild(createTarget());
@@ -79,8 +86,20 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
     waypointsNode = createWaypoints();
     rollingMap->addChild(waypointsNode);
 
+#ifdef QGC_LIBFREENECT_ENABLED
+    freenect.reset(new Freenect());
+    freenect->init();
+#endif
+
+    // generate RGBD model
+    rgbd3DNode = createRGBD3D();
+    egocentricMap->addChild(rgbd3DNode);
+
     setupHUD();
 
+    // find available vehicle models in models folder
+    vehicleModels = findVehicleModels();
+
     buildLayout();
 
     connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
@@ -92,59 +111,6 @@ Pixhawk3DWidget::~Pixhawk3DWidget()
 
 }
 
-void
-Pixhawk3DWidget::buildLayout(void)
-{
-    QCheckBox* gridCheckBox = new QCheckBox(this);
-    gridCheckBox->setText("Grid");
-    gridCheckBox->setChecked(displayGrid);
-
-    QCheckBox* trailCheckBox = new QCheckBox(this);
-    trailCheckBox->setText("Trail");
-    trailCheckBox->setChecked(displayTrail);
-
-    QCheckBox* waypointsCheckBox = new QCheckBox(this);
-    waypointsCheckBox->setText("Waypoints");
-    waypointsCheckBox->setChecked(displayWaypoints);
-
-    targetButton = new QPushButton(this);
-    targetButton->setCheckable(true);
-    targetButton->setChecked(false);
-    targetButton->setIcon(QIcon(QString::fromUtf8(":/images/status/weather-clear.svg")));
-
-    QPushButton* recenterButton = new QPushButton(this);
-    recenterButton->setText("Recenter Camera");
-
-    QCheckBox* followCameraCheckBox = new QCheckBox(this);
-    followCameraCheckBox->setText("Follow Camera");
-    followCameraCheckBox->setChecked(followCamera);
-
-    QGridLayout* layout = new QGridLayout(this);
-    layout->setMargin(0);
-    layout->setSpacing(2);
-    layout->addWidget(gridCheckBox, 1, 0);
-    layout->addWidget(trailCheckBox, 1, 1);
-    layout->addWidget(waypointsCheckBox, 1, 2);
-    layout->addItem(new QSpacerItem(20, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 3);
-    layout->addWidget(targetButton, 1, 4);
-    layout->addItem(new QSpacerItem(20, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 5);
-    layout->addWidget(recenterButton, 1, 6);
-    layout->addWidget(followCameraCheckBox, 1, 7);
-    layout->setRowStretch(0, 100);
-    layout->setRowStretch(1, 1);
-    setLayout(layout);
-
-    connect(gridCheckBox, SIGNAL(stateChanged(int)),
-            this, SLOT(showGrid(int)));
-    connect(trailCheckBox, SIGNAL(stateChanged(int)),
-            this, SLOT(showTrail(int)));
-    connect(waypointsCheckBox, SIGNAL(stateChanged(int)),
-            this, SLOT(showWaypoints(int)));
-    connect(recenterButton, SIGNAL(clicked()), this, SLOT(recenter()));
-    connect(followCameraCheckBox, SIGNAL(stateChanged(int)),
-            this, SLOT(toggleFollowCamera(int)));
-}
-
 /**
  *
  * @param uas the UAS/MAV to monitor/display with the HUD
@@ -204,6 +170,14 @@ Pixhawk3DWidget::showWaypoints(int state)
     }
 }
 
+void
+Pixhawk3DWidget::selectVehicleModel(int index)
+{
+    egocentricMap->removeChild(vehicleModel);
+    vehicleModel = vehicleModels.at(index);
+    egocentricMap->addChild(vehicleModel);
+}
+
 void
 Pixhawk3DWidget::recenter(void)
 {
@@ -230,6 +204,123 @@ Pixhawk3DWidget::toggleFollowCamera(int32_t state)
         followCamera = false;
     }
 }
+#include <osgDB/WriteFile>
+QVector< osg::ref_ptr<osg::Node> >
+Pixhawk3DWidget::findVehicleModels(void)
+{
+    QDir directory("models");
+    QStringList files = directory.entryList(QStringList("*.osg"), QDir::Files);
+
+    QVector< osg::ref_ptr<osg::Node> > nodes;
+
+    // add Pixhawk Bravo model
+    nodes.push_back(PixhawkCheetahGeode::instance());
+
+    // add all other models in folder
+    for (int i = 0; i < files.size(); ++i)
+    {
+        osg::ref_ptr<osg::Node> node =
+                osgDB::readNodeFile(directory.absoluteFilePath(files[i]).toStdString().c_str());
+
+        if (node)
+        {
+
+        nodes.push_back(node);
+        }
+        else
+        {
+            printf(QString("ERROR: Could not load file " + directory.absoluteFilePath(files[i]) + "\n").toStdString().c_str());
+        }
+    }
+
+//    QStringList dirs = directory.entryList(QDir::Dirs);
+//    // Add models in subfolders
+//    for (int i = 0; i < dirs.size(); ++i)
+//    {
+//        // Handling the current directory
+//        QStringList currFiles = QDir(dirs[i]).entryList(QStringList("*.ac"), QDir::Files);
+
+//        // Load the file
+//        osg::ref_ptr<osg::Node> node =
+//                osgDB::readNodeFile(directory.absoluteFilePath(currFiles.first()).toStdString().c_str());
+
+//        if (node)
+//        {
+
+//        nodes.push_back(node);
+//        }
+//        else
+//        {
+//            printf(QString("ERROR: Could not load file " + directory.absoluteFilePath(files[i]) + "\n").toStdString().c_str());
+//        }
+//    }
+
+    return nodes;
+}
+
+void
+Pixhawk3DWidget::buildLayout(void)
+{
+    QCheckBox* gridCheckBox = new QCheckBox(this);
+    gridCheckBox->setText("Grid");
+    gridCheckBox->setChecked(displayGrid);
+
+    QCheckBox* trailCheckBox = new QCheckBox(this);
+    trailCheckBox->setText("Trail");
+    trailCheckBox->setChecked(displayTrail);
+
+    QCheckBox* waypointsCheckBox = new QCheckBox(this);
+    waypointsCheckBox->setText("Waypoints");
+    waypointsCheckBox->setChecked(displayWaypoints);
+
+    QLabel* modelLabel = new QLabel("Vehicle Model", this);
+    QComboBox* modelComboBox = new QComboBox(this);
+    for (int i = 0; i < vehicleModels.size(); ++i)
+    {
+        modelComboBox->addItem(vehicleModels[i]->getName().c_str());
+    }
+
+    targetButton = new QPushButton(this);
+    targetButton->setCheckable(true);
+    targetButton->setChecked(false);
+    targetButton->setIcon(QIcon(QString::fromUtf8(":/images/status/weather-clear.svg")));
+
+    QPushButton* recenterButton = new QPushButton(this);
+    recenterButton->setText("Recenter Camera");
+
+    QCheckBox* followCameraCheckBox = new QCheckBox(this);
+    followCameraCheckBox->setText("Follow Camera");
+    followCameraCheckBox->setChecked(followCamera);
+
+    QGridLayout* layout = new QGridLayout(this);
+    layout->setMargin(0);
+    layout->setSpacing(2);
+    layout->addWidget(gridCheckBox, 1, 0);
+    layout->addWidget(trailCheckBox, 1, 1);
+    layout->addWidget(waypointsCheckBox, 1, 2);
+    layout->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 3);
+    layout->addWidget(modelLabel, 1, 4);
+    layout->addWidget(modelComboBox, 1, 5);
+    layout->addWidget(targetButton, 1, 6);
+    layout->addItem(new QSpacerItem(10, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 7);
+    layout->addWidget(recenterButton, 1, 8);
+    layout->addWidget(followCameraCheckBox, 1, 9);
+    layout->setRowStretch(0, 100);
+    layout->setRowStretch(1, 1);
+    setLayout(layout);
+
+    connect(gridCheckBox, SIGNAL(stateChanged(int)),
+            this, SLOT(showGrid(int)));
+    connect(trailCheckBox, SIGNAL(stateChanged(int)),
+            this, SLOT(showTrail(int)));
+    connect(waypointsCheckBox, SIGNAL(stateChanged(int)),
+            this, SLOT(showWaypoints(int)));
+    connect(modelComboBox, SIGNAL(currentIndexChanged(int)),
+            this, SLOT(selectVehicleModel(int)));
+    connect(recenterButton, SIGNAL(clicked()), this, SLOT(recenter()));
+    connect(followCameraCheckBox, SIGNAL(stateChanged(int)),
+            this, SLOT(toggleFollowCamera(int)));
+}
 
 void
 Pixhawk3DWidget::display(void)
@@ -271,22 +362,47 @@ Pixhawk3DWidget::display(void)
                                          robotPitch, osg::Vec3f(1.0f, 0.0f, 0.0f),
                                          robotRoll, osg::Vec3f(0.0f, 1.0f, 0.0f)));
 
-    updateHUD(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw);
     updateTrail(robotX, robotY, robotZ);
     updateTarget();
     updateWaypoints();
+#ifdef QGC_LIBFREENECT_ENABLED
+    updateRGBD();
+#endif
+    updateHUD(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw);
 
     // set node visibility
     rollingMap->setChildValue(gridNode, displayGrid);
     rollingMap->setChildValue(trailNode, displayTrail);
     rollingMap->setChildValue(targetNode, displayTarget);
     rollingMap->setChildValue(waypointsNode, displayWaypoints);
+    egocentricMap->setChildValue(rgbd3DNode, displayRGBD3D);
+    hudGroup->setChildValue(rgb2DGeode, displayRGBD2D);
+    hudGroup->setChildValue(depth2DGeode, displayRGBD2D);
 
     lastRobotX = robotX;
     lastRobotY = robotY;
     lastRobotZ = robotZ;
 }
 
+void
+Pixhawk3DWidget::keyPressEvent(QKeyEvent* event)
+{
+    if (!event->text().isEmpty())
+    {
+        switch (*(event->text().toAscii().data()))
+        {
+        case '1':
+            displayRGBD2D = !displayRGBD2D;
+            break;
+        case '2':
+            displayRGBD3D = !displayRGBD3D;
+            break;
+        }
+    }
+
+    Q3DWidget::keyPressEvent(event);
+}
+
 void
 Pixhawk3DWidget::mousePressEvent(QMouseEvent* event)
 {
@@ -393,6 +509,7 @@ Pixhawk3DWidget::createTrail(void)
     return geode;
 }
 
+#ifdef QGC_OSGEARTH_ENABLED
 osg::ref_ptr<osgEarth::MapNode>
 Pixhawk3DWidget::createMap(void)
 {
@@ -401,6 +518,7 @@ Pixhawk3DWidget::createMap(void)
 
     return node;
 }
+#endif
 
 osg::ref_ptr<osg::Node>
 Pixhawk3DWidget::createTarget(void)
@@ -421,72 +539,111 @@ Pixhawk3DWidget::createWaypoints(void)
     return group;
 }
 
+osg::ref_ptr<osg::Geode>
+Pixhawk3DWidget::createRGBD3D(void)
+{
+    int frameSize = 640 * 480;
+
+    osg::ref_ptr<osg::Geode> geode(new osg::Geode);
+    osg::ref_ptr<osg::Geometry> geometry(new osg::Geometry);
+
+    osg::ref_ptr<osg::Vec3Array> vertices(new osg::Vec3Array(frameSize));
+    geometry->setVertexArray(vertices);
+
+    osg::ref_ptr<osg::Vec4Array> colors(new osg::Vec4Array(frameSize));
+    geometry->setColorArray(colors);
+    geometry->setColorBinding(osg::Geometry::BIND_PER_VERTEX);
+    geometry->setUseDisplayList(false);
+
+    geode->addDrawable(geometry);
+
+    return geode;
+}
+
 void
 Pixhawk3DWidget::setupHUD(void)
 {
-    osg::ref_ptr<osg::Vec3Array> hudBackgroundVertices(new osg::Vec3Array);
-    hudBackgroundVertices->push_back(osg::Vec3(0, height(), -1));
-    hudBackgroundVertices->push_back(osg::Vec3(width(), height(), -1));
-    hudBackgroundVertices->push_back(osg::Vec3(width(), height() - 30, -1));
-    hudBackgroundVertices->push_back(osg::Vec3(0, height() - 30, -1));
-    hudBackgroundVertices->push_back(osg::Vec3(0, 0, -1));
-    hudBackgroundVertices->push_back(osg::Vec3(width(), 0, -1));
-    hudBackgroundVertices->push_back(osg::Vec3(width(), 25, -1));
-    hudBackgroundVertices->push_back(osg::Vec3(0, 25, -1));
-
-    osg::ref_ptr<osg::DrawElementsUInt> hudTopBackgroundIndices(
-            new osg::DrawElementsUInt(osg::PrimitiveSet::POLYGON, 0));
-    hudTopBackgroundIndices->push_back(0);
-    hudTopBackgroundIndices->push_back(1);
-    hudTopBackgroundIndices->push_back(2);
-    hudTopBackgroundIndices->push_back(3);
-
-    osg::ref_ptr<osg::DrawElementsUInt> hudBottomBackgroundIndices(
-            new osg::DrawElementsUInt(osg::PrimitiveSet::POLYGON, 0));
-    hudBottomBackgroundIndices->push_back(4);
-    hudBottomBackgroundIndices->push_back(5);
-    hudBottomBackgroundIndices->push_back(6);
-    hudBottomBackgroundIndices->push_back(7);
-
     osg::ref_ptr<osg::Vec4Array> hudColors(new osg::Vec4Array);
     hudColors->push_back(osg::Vec4(0.0f, 0.0f, 0.0f, 0.2f));
 
     hudBackgroundGeometry = new osg::Geometry;
-    hudBackgroundGeometry->addPrimitiveSet(hudTopBackgroundIndices);
-    hudBackgroundGeometry->addPrimitiveSet(hudBottomBackgroundIndices);
-    hudBackgroundGeometry->setVertexArray(hudBackgroundVertices);
+    hudBackgroundGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON,
+                                                               0, 4));
+    hudBackgroundGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POLYGON,
+                                                               4, 4));
     hudBackgroundGeometry->setColorArray(hudColors);
     hudBackgroundGeometry->setColorBinding(osg::Geometry::BIND_OVERALL);
-
-    hudGeode->addDrawable(hudBackgroundGeometry);
+    hudBackgroundGeometry->setUseDisplayList(false);
 
     statusText = new osgText::Text;
     statusText->setCharacterSize(11);
     statusText->setFont("images/Vera.ttf");
     statusText->setAxisAlignment(osgText::Text::SCREEN);
-    statusText->setPosition(osg::Vec3(10, height() - 10, -1.5));
     statusText->setColor(osg::Vec4(255, 255, 255, 1));
 
-    hudGeode->addDrawable(statusText);
+    resizeHUD();
+
+    osg::ref_ptr<osg::Geode> statusGeode = new osg::Geode;
+    statusGeode->addDrawable(hudBackgroundGeometry);
+    statusGeode->addDrawable(statusText);
+    hudGroup->addChild(statusGeode);
+
+    rgbImage = new osg::Image;
+    rgb2DGeode = new ImageWindowGeode("RGB Image",
+                                      osg::Vec4(0.0f, 0.0f, 0.1f, 1.0f),
+                                      rgbImage);
+    hudGroup->addChild(rgb2DGeode);
+
+    depthImage = new osg::Image;
+    depth2DGeode = new ImageWindowGeode("Depth Image",
+                                        osg::Vec4(0.0f, 0.0f, 0.1f, 1.0f),
+                                        depthImage);
+    hudGroup->addChild(depth2DGeode);
 }
 
 void
-Pixhawk3DWidget::updateHUD(float robotX, float robotY, float robotZ,
-                         float robotRoll, float robotPitch, float robotYaw)
+Pixhawk3DWidget::resizeHUD(void)
 {
-    osg::ref_ptr<osg::Vec3Array> hudBackgroundVertices(new osg::Vec3Array);
-    hudBackgroundVertices->push_back(osg::Vec3(0, height(), -1));
-    hudBackgroundVertices->push_back(osg::Vec3(width(), height(), -1));
-    hudBackgroundVertices->push_back(osg::Vec3(width(), height() - 30, -1));
-    hudBackgroundVertices->push_back(osg::Vec3(0, height() - 30, -1));
-    hudBackgroundVertices->push_back(osg::Vec3(0, 0, -1));
-    hudBackgroundVertices->push_back(osg::Vec3(width(), 0, -1));
-    hudBackgroundVertices->push_back(osg::Vec3(width(), 25, -1));
-    hudBackgroundVertices->push_back(osg::Vec3(0, 25, -1));
-    hudBackgroundGeometry->setVertexArray(hudBackgroundVertices);
+    int topHUDHeight = 30;
+    int bottomHUDHeight = 25;
+
+    osg::Vec3Array* vertices = static_cast<osg::Vec3Array*>(hudBackgroundGeometry->getVertexArray());
+    if (vertices == NULL || vertices->size() != 8)
+    {
+        osg::ref_ptr<osg::Vec3Array> newVertices = new osg::Vec3Array(8);
+        hudBackgroundGeometry->setVertexArray(newVertices);
+
+        vertices = static_cast<osg::Vec3Array*>(hudBackgroundGeometry->getVertexArray());
+    }
+
+    (*vertices)[0] = osg::Vec3(0, height(), -1);
+    (*vertices)[1] = osg::Vec3(width(), height(), -1);
+    (*vertices)[2] = osg::Vec3(width(), height() - topHUDHeight, -1);
+    (*vertices)[3] = osg::Vec3(0, height() - topHUDHeight, -1);
+    (*vertices)[4] = osg::Vec3(0, 0, -1);
+    (*vertices)[5] = osg::Vec3(width(), 0, -1);
+    (*vertices)[6] = osg::Vec3(width(), bottomHUDHeight, -1);
+    (*vertices)[7] = osg::Vec3(0, bottomHUDHeight, -1);
 
     statusText->setPosition(osg::Vec3(10, height() - 20, -1.5));
 
+    if (rgb2DGeode.valid() && depth2DGeode.valid())
+    {
+        int windowWidth = (width() - 20) / 2;
+        int windowHeight = 3 * windowWidth / 4;
+        rgb2DGeode->setAttributes(10, (height() - windowHeight) / 2,
+                                  windowWidth, windowHeight);
+        depth2DGeode->setAttributes(width() / 2, (height() - windowHeight) / 2,
+                                    windowWidth, windowHeight);
+    }
+}
+
+void
+Pixhawk3DWidget::updateHUD(float robotX, float robotY, float robotZ,
+                           float robotRoll, float robotPitch, float robotYaw)
+{
+    resizeHUD();
+
     std::pair<double,double> cursorPosition =
             getGlobalCursorPosition(getMouseX(), getMouseY(), -robotZ);
 
@@ -502,6 +659,21 @@ Pixhawk3DWidget::updateHUD(float robotX, float robotY, float robotZ,
             " Cursor [" << cursorPosition.first <<
             " " << cursorPosition.second << "]";
     statusText->setText(oss.str());
+
+    if (!rgb.isNull())
+    {
+        rgbImage->setImage(640, 480, 1,
+                           GL_RGB, GL_RGB, GL_UNSIGNED_BYTE,
+                           reinterpret_cast<unsigned char *>(rgb->data()),
+                           osg::Image::NO_DELETE);
+        rgbImage->dirty();
+
+        depthImage->setImage(640, 480, 1,
+                             GL_RGB, GL_RGB, GL_UNSIGNED_BYTE,
+                             reinterpret_cast<unsigned char *>(coloredDepth->data()),
+                             osg::Image::NO_DELETE);
+        depthImage->dirty();
+    }
 }
 
 void
@@ -637,6 +809,14 @@ Pixhawk3DWidget::updateWaypoints(void)
     }
 }
 
+#ifdef QGC_LIBFREENECT_ENABLED
+void
+Pixhawk3DWidget::updateRGBD(void)
+{
+    rgb = freenect->getRgbData();
+    coloredDepth = freenect->getColoredDepthData();
+}
+#endif
 
 void
 Pixhawk3DWidget::markTarget(void)

src/ui/map3D/Pixhawk3DWidget.h

@@ -33,7 +33,15 @@
 #define PIXHAWK3DWIDGET_H
 
 #include <osgText/Text>
+#ifdef QGC_OSGEARTH_ENABLED
 #include <osgEarth/MapNode>
+#endif
+
+#include "ImageWindowGeode.h"
+
+#ifdef QGC_LIBFREENECT_ENABLED
+#include "Freenect.h"
+#endif
 
 #include "Q3DWidget.h"
 
@@ -50,10 +58,6 @@ public:
     explicit Pixhawk3DWidget(QWidget* parent = 0);
     ~Pixhawk3DWidget();
 
-    void buildLayout(void);
-
-    double getTime(void) const;
-
 public slots:
     void setActiveUAS(UASInterface* uas);
 
@@ -61,11 +65,15 @@ private slots:
     void showGrid(int state);
     void showTrail(int state);
     void showWaypoints(int state);
+    void selectVehicleModel(int index);
     void recenter(void);
     void toggleFollowCamera(int state);
 
 protected:
+    QVector< osg::ref_ptr<osg::Node> > findVehicleModels(void);
+    void buildLayout(void);
     virtual void display(void);
+    virtual void keyPressEvent(QKeyEvent* event);
     virtual void mousePressEvent(QMouseEvent* event);
 
     UASInterface* uas;
@@ -73,17 +81,26 @@ protected:
 private:
     osg::ref_ptr<osg::Geode> createGrid(void);
     osg::ref_ptr<osg::Geode> createTrail(void);
+
+#ifdef QGC_OSGEARTH_ENABLED
     osg::ref_ptr<osgEarth::MapNode> createMap(void);
+#endif
+
     osg::ref_ptr<osg::Node> createTarget(void);
     osg::ref_ptr<osg::Group> createWaypoints(void);
+    osg::ref_ptr<osg::Geode> createRGBD3D(void);
 
     void setupHUD(void);
+    void resizeHUD(void);
 
     void updateHUD(float robotX, float robotY, float robotZ,
                    float robotRoll, float robotPitch, float robotYaw);
     void updateTrail(float robotX, float robotY, float robotZ);
     void updateTarget(void);
     void updateWaypoints(void);
+#ifdef QGC_LIBFREENECT_ENABLED
+    void updateRGBD(void);
+#endif
 
     void markTarget(void);
 
@@ -91,22 +108,39 @@ private:
     bool displayTrail;
     bool displayTarget;
     bool displayWaypoints;
+    bool displayRGBD2D;
+    bool displayRGBD3D;
 
     bool followCamera;
 
     osg::ref_ptr<osg::Vec3Array> trailVertices;
     QVarLengthArray<osg::Vec3, 10000> trail;
 
+    osg::ref_ptr<osg::Node> vehicleModel;
     osg::ref_ptr<osg::Geometry> hudBackgroundGeometry;
     osg::ref_ptr<osgText::Text> statusText;
+    osg::ref_ptr<ImageWindowGeode> rgb2DGeode;
+    osg::ref_ptr<ImageWindowGeode> depth2DGeode;
+    osg::ref_ptr<osg::Image> rgbImage;
+    osg::ref_ptr<osg::Image> depthImage;
     osg::ref_ptr<osg::Geode> gridNode;
     osg::ref_ptr<osg::Geode> trailNode;
     osg::ref_ptr<osg::Geometry> trailGeometry;
     osg::ref_ptr<osg::DrawArrays> trailDrawArrays;
+#ifdef QGC_OSGEARTH_ENABLED
     osg::ref_ptr<osgEarth::MapNode> mapNode;
+#endif
     osg::ref_ptr<osg::Geode> targetNode;
     osg::ref_ptr<osg::PositionAttitudeTransform> targetPosition;
     osg::ref_ptr<osg::Group> waypointsNode;
+    osg::ref_ptr<osg::Geode> rgbd3DNode;
+#ifdef QGC_LIBFREENECT_ENABLED
+    QScopedPointer<Freenect> freenect;
+#endif
+    QSharedPointer<QByteArray> rgb;
+    QSharedPointer<QByteArray> coloredDepth;
+
+    QVector< osg::ref_ptr<osg::Node> > vehicleModels;
 
     QPushButton* targetButton;
 

src/ui/map3D/PixhawkCheetahGeode.cc

@@ -59358,6 +59358,7 @@ osg::ref_ptr<osg::Geode>
 PixhawkCheetahGeode::create(float red, float green, float blue)
 {
     osg::ref_ptr<osg::Geode> geode(new osg::Geode());
+    geode->setName("Pixhawk Bravo");
     osg::ref_ptr<osg::Geometry> geometry(new osg::Geometry());
     geode->addDrawable(geometry.get());
 

src/ui/map3D/Q3DWidget.cc

@@ -34,10 +34,7 @@ This file is part of the QGROUNDCONTROL project
 #include <osg/Geometry>
 #include <osg/LineWidth>
 #include <osg/MatrixTransform>
-
-static const float KEY_ROTATE_AMOUNT = 5.0f;
-static const float KEY_MOVE_AMOUNT   = 10.0f;
-static const float KEY_ZOOM_AMOUNT   = 5.0f;
+#include <Carbon/Carbon.h>
 
 Q3DWidget::Q3DWidget(QWidget* parent)
     : QGLWidget(parent)
@@ -47,7 +44,7 @@ Q3DWidget::Q3DWidget(QWidget* parent)
     , egocentricMap(new osg::Switch())
     , robotPosition(new osg::PositionAttitudeTransform())
     , robotAttitude(new osg::PositionAttitudeTransform())
-    , hudGeode(new osg::Geode())
+    , hudGroup(new osg::Switch())
     , hudProjectionMatrix(new osg::Projection)
 {
     // set initial camera parameters
@@ -94,7 +91,7 @@ Q3DWidget::init(float fps)
     egocentricMap->addChild(createRobot());
 
     // set up camera control
-    cameraManipulator = new GCManipulator;
+    cameraManipulator = new GCManipulator();
     setCameraManipulator(cameraManipulator);
     cameraManipulator->setMinZoomRange(cameraParams.minZoomRange);
     cameraManipulator->setDistance(cameraParams.minZoomRange * 2.0);
@@ -159,11 +156,12 @@ Q3DWidget::createHUD(void)
     hudModelViewMatrix->setReferenceFrame(osg::Transform::ABSOLUTE_RF);
 
     hudProjectionMatrix->addChild(hudModelViewMatrix);
-    hudModelViewMatrix->addChild(hudGeode);
+    hudModelViewMatrix->addChild(hudGroup);
 
     osg::ref_ptr<osg::StateSet> hudStateSet(new osg::StateSet);
-    hudGeode->setStateSet(hudStateSet);
+    hudGroup->setStateSet(hudStateSet);
     hudStateSet->setMode(GL_DEPTH_TEST, osg::StateAttribute::OFF);
+    hudStateSet->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
     hudStateSet->setRenderingHint(osg::StateSet::TRANSPARENT_BIN);
     hudStateSet->setRenderBinDetails(11, "RenderBin");
 

src/ui/map3D/Q3DWidget.h

@@ -249,7 +249,7 @@ protected:
     osg::ref_ptr<osg::PositionAttitudeTransform> robotPosition;
     osg::ref_ptr<osg::PositionAttitudeTransform> robotAttitude;
 
-    osg::ref_ptr<osg::Geode> hudGeode; /**< A geode which contains renderable HUD objects. */
+    osg::ref_ptr<osg::Switch> hudGroup; /**< A group which contains renderable HUD objects. */
     osg::ref_ptr<osg::Projection> hudProjectionMatrix; /**< An orthographic projection matrix for HUD display. */
 
     osg::ref_ptr<osgViewer::GraphicsWindowEmbedded> osgGW; /**< A class which manages OSG graphics windows and events. */

src/ui/map3D/Q3DWidgetFactory.cc

@@ -32,16 +32,25 @@ This file is part of the QGROUNDCONTROL project
 #include "Q3DWidgetFactory.h"
 
 #include "Pixhawk3DWidget.h"
+#ifdef QGC_OSGEARTH_ENABLED
+#include "QMap3D.h"
+#endif
 
-QPointer<Q3DWidget>
+QPointer<QWidget>
 Q3DWidgetFactory::get(const std::string& type)
 {
     if (type == "PIXHAWK")
     {
-        return QPointer<Q3DWidget>(new Pixhawk3DWidget);
+        return QPointer<QWidget>(new Pixhawk3DWidget());
     }
+#ifdef QGC_OSGEARTH_ENABLED
+    else if (type == "MAP3D")
+    {
+        return QPointer<QWidget>(new QMap3D());
+    }
+#endif
     else
     {
-        return QPointer<Q3DWidget>(new Q3DWidget);
+        return QPointer<QWidget>(new Q3DWidget());
     }
 }

src/ui/map3D/Q3DWidgetFactory.h

@@ -50,7 +50,7 @@ public:
      * @return A smart pointer to the Q3DWidget instance.
      */
 
-    static QPointer<Q3DWidget> get(const std::string& type);
+    static QPointer<QWidget> get(const std::string& type);
 };
 
 #endif // Q3DWIDGETFACTORY_H

src/ui/map3D/QMap3D.cc

@@ -21,6 +21,8 @@ This file is part of the QGROUNDCONTROL project
 
 ======================================================================*/
 
+#ifdef QGC_OSGEARTH_ENABLED
+
 /**
  * @file
  *   @brief Definition of the class QMap3D.
@@ -62,3 +64,5 @@ void QMap3D::on_pushButton_vehicle_clicked()
     graphicsView->getSceneData()->asGroup()->addChild(osgDB::readNodeFile(vehicleName.toStdString()));
     graphicsView->updateCamera();
 }
+
+#endif

src/ui/map3D/QMap3D.h

@@ -29,6 +29,8 @@ This file is part of the QGROUNDCONTROL project
  *
  */
 
+#ifdef QGC_OSGEARTH_ENABLED
+
 #ifndef QMAP3D_H
 #define QMAP3D_H
 
@@ -52,3 +54,4 @@ public slots:
 
 #endif // QMAP3D_H
 
+#endif

src/ui/map3D/QOSGWidget.cc

@@ -18,23 +18,18 @@
 
 #include "QOSGWidget.h"
 
+#ifdef Q_OS_MACX
+
+#endif
+
 QOSGWidget::QOSGWidget( QWidget * parent, const char * name, WindowFlags f, bool overrideTraits):
-#if USE_QT4
     QWidget(parent, f), _overrideTraits (overrideTraits)
-#else
-    QWidget(parent, name, f), _overrideTraits (overrideTraits)
-#endif
 {
     createContext();
     
-
-#if USE_QT4
     setAttribute(Qt::WA_PaintOnScreen);
     setAttribute(Qt::WA_NoSystemBackground);
-    setFocusPolicy(Qt::ClickFocus);
-#else
-    setBackgroundMode(Qt::NoBackground);
-#endif    
+    setFocusPolicy(Qt::ClickFocus);   
 }
 
 void QOSGWidget::createContext()
@@ -106,9 +101,7 @@ void QOSGWidget::destroyEvent(bool destroyWindow, bool destroySubWindows)
 
 void QOSGWidget::closeEvent( QCloseEvent * event )
 {
-#ifndef USE_QT4
     event->accept();
-#endif
 
     _gw->getEventQueue()->closeWindow();
 }
@@ -123,20 +116,12 @@ void QOSGWidget::resizeEvent( QResizeEvent * event )
 
 void QOSGWidget::keyPressEvent( QKeyEvent* event )
 {
-#if USE_QT4
     _gw->getEventQueue()->keyPress( (osgGA::GUIEventAdapter::KeySymbol) *(event->text().toAscii().data() ) );
-#else
-    _gw->getEventQueue()->keyPress( (osgGA::GUIEventAdapter::KeySymbol) event->ascii() );
-#endif
 }
 
 void QOSGWidget::keyReleaseEvent( QKeyEvent* event )
 {
-#if USE_QT4
     int c = *event->text().toAscii().data();
-#else
-    int c = event->ascii();
-#endif
 
     _gw->getEventQueue()->keyRelease( (osgGA::GUIEventAdapter::KeySymbol) (c) );
 }
@@ -229,8 +214,6 @@ void CompositeViewerQOSG::RemoveView()
   Tile();
 }
 
-
-#if USE_QT4
 // we use this wrapper for CompositeViewer ONLY because of the timer
 // NOTE: this is a workaround because we're not using QT's moc precompiler here.
 //
@@ -259,7 +242,6 @@ class QViewerTimer : public QWidget
         QTimer _timer;
 
 };
-#endif
 
 void setupHandlers(osgViewer::View * viewer)
 {

src/ui/map3D/QOSGWidget.h

@@ -19,31 +19,17 @@
 #include <osg/Config>
 
 #if defined(_MSC_VER) && defined(OSG_DISABLE_MSVC_WARNINGS)
-    // disable warning "'QtConcurrent::BlockSizeManager' : assignment operator could not be generated"
-    #pragma warning( disable : 4512 )
-#endif
-
-#if USE_QT4
-
-    #include <QtCore/QString>
-    #include <QtCore/QTimer>
-    #include <QtGui/QKeyEvent>
-    #include <QtGui/QApplication>
-    #include <QtGui/QtGui>
-    #include <QtGui/QWidget>
-    using Qt::WindowFlags;
-
-#else
-
-    class QWidget;
-    #include <qtimer.h>
-    #include <qgl.h>
-    #include <qapplication.h>
-
-    #define WindowFlags WFlags
-
+// disable warning "'QtConcurrent::BlockSizeManager' : assignment operator could not be generated"
+#pragma warning( disable : 4512 )
 #endif
 
+#include <QtCore/QString>
+#include <QtCore/QTimer>
+#include <QtGui/QKeyEvent>
+#include <QtGui/QApplication>
+#include <QtGui/QtGui>
+#include <QtGui/QWidget>
+using Qt::WindowFlags;
 
 #include <osgViewer/Viewer>
 #include <osgViewer/CompositeViewer>
@@ -85,119 +71,119 @@ typedef osgViewer::GraphicsWindowX11::WindowData WindowData;
 
 class QOSGWidget : public QWidget
 {
-    public:
+public:
 
-        QOSGWidget( QWidget * parent = 0, const char * name = 0, WindowFlags f = 0, bool overrideTraits = false);
+    QOSGWidget( QWidget * parent = 0, const char * name = 0, WindowFlags f = 0, bool overrideTraits = false);
 
-        virtual ~QOSGWidget() {}
+    virtual ~QOSGWidget() {}
 
-        osgViewer::GraphicsWindow* getGraphicsWindow() { return _gw.get(); }
-        const osgViewer::GraphicsWindow* getGraphicsWindow() const { return _gw.get(); }
+    osgViewer::GraphicsWindow* getGraphicsWindow() { return _gw.get(); }
+    const osgViewer::GraphicsWindow* getGraphicsWindow() const { return _gw.get(); }
 
-    protected:
+protected:
 
-        void init();
-        void createContext();
+    void init();
+    void createContext();
 
-        //  The GraphincsWindowWin32 implementation already takes care of message handling.
-        //  We don't want to relay these on Windows, it will just cause duplicate messages
-        //  with further problems downstream (i.e. not being able to throw the trackball
+    //  The GraphincsWindowWin32 implementation already takes care of message handling.
+    //  We don't want to relay these on Windows, it will just cause duplicate messages
+    //  with further problems downstream (i.e. not being able to throw the trackball
 #ifndef WIN32 
-        virtual void mouseDoubleClickEvent ( QMouseEvent * event );
-        virtual void closeEvent( QCloseEvent * event );
-        virtual void destroyEvent( bool destroyWindow = true, bool destroySubWindows = true);
-        virtual void resizeEvent( QResizeEvent * event );
-        virtual void keyPressEvent( QKeyEvent* event );
-        virtual void keyReleaseEvent( QKeyEvent* event );
-        virtual void mousePressEvent( QMouseEvent* event );
-        virtual void mouseReleaseEvent( QMouseEvent* event );
-        virtual void mouseMoveEvent( QMouseEvent* event );
+    virtual void mouseDoubleClickEvent ( QMouseEvent * event );
+    virtual void closeEvent( QCloseEvent * event );
+    virtual void destroyEvent( bool destroyWindow = true, bool destroySubWindows = true);
+    virtual void resizeEvent( QResizeEvent * event );
+    virtual void keyPressEvent( QKeyEvent* event );
+    virtual void keyReleaseEvent( QKeyEvent* event );
+    virtual void mousePressEvent( QMouseEvent* event );
+    virtual void mouseReleaseEvent( QMouseEvent* event );
+    virtual void mouseMoveEvent( QMouseEvent* event );
 #endif
-        osg::ref_ptr<osgViewer::GraphicsWindow> _gw;
+    osg::ref_ptr<osgViewer::GraphicsWindow> _gw;
     bool _overrideTraits;
 };
 
 class ViewerQOSG : public osgViewer::Viewer, public QOSGWidget
 {
-    public:
+public:
 
-        ViewerQOSG(QWidget * parent = 0, const char * name = 0, WindowFlags f = 0, int fps = 20):
+    ViewerQOSG(QWidget * parent = 0, const char * name = 0, WindowFlags f = 0, int fps = 20):
             QOSGWidget( parent, name, f )
-        {
-            setThreadingModel(osgViewer::Viewer::SingleThreaded);
+    {
+        setThreadingModel(osgViewer::Viewer::SingleThreaded);
 
-            connect(&_timer, SIGNAL(timeout()), this, SLOT(update()));
-            _timer.start(1000.0f/fps);
-        }
+        connect(&_timer, SIGNAL(timeout()), this, SLOT(update()));
+        _timer.start(1000.0f/fps);
+    }
 
-        void updateCamera()
-        {
-            getCamera()->setViewport(new osg::Viewport(0,0,width(),height()));
-            getCamera()->setProjectionMatrixAsPerspective(30.0f, 1.0f , static_cast<double>(width())/static_cast<double>(height()), 10000.0f);
-            getCamera()->setGraphicsContext(getGraphicsWindow());
-        }
+    void updateCamera()
+    {
+        getCamera()->setViewport(new osg::Viewport(0,0,width(),height()));
+        getCamera()->setProjectionMatrixAsPerspective(30.0f, 1.0f , static_cast<double>(width())/static_cast<double>(height()), 10000.0f);
+        getCamera()->setGraphicsContext(getGraphicsWindow());
+    }
 
-        virtual void paintEvent( QPaintEvent * event ) { frame(); }
+    virtual void paintEvent( QPaintEvent * event ) { frame(); }
 
-    protected:
+protected:
 
-        QTimer _timer;
+    QTimer _timer;
 };
 
 
 class CompositeViewerQOSG : public osgViewer::CompositeViewer, public QOSGWidget
 {
-    public:
-        CompositeViewerQOSG(QWidget * parent = 0, const char * name = 0, WindowFlags f = 0, int fps = 20)
+public:
+    CompositeViewerQOSG(QWidget * parent = 0, const char * name = 0, WindowFlags f = 0, int fps = 20)
         : QOSGWidget( parent, name, f )
-        {
-          setThreadingModel(osgViewer::CompositeViewer::SingleThreaded);
+    {
+        setThreadingModel(osgViewer::CompositeViewer::SingleThreaded);
 
-          connect(&_timer, SIGNAL(timeout()), this, SLOT(repaint()));
+        connect(&_timer, SIGNAL(timeout()), this, SLOT(repaint()));
 
-          // The composite viewer needs at least one view to work
-          // Create a dummy view with a zero sized viewport and no
-          // scene to keep the viewer alive.
-          osgViewer::View * pView = new osgViewer::View;
-          pView->getCamera()->setGraphicsContext( getGraphicsWindow() );
-          pView->getCamera()->setViewport( 0, 0, 0, 0 );
-          addView( pView );
+        // The composite viewer needs at least one view to work
+        // Create a dummy view with a zero sized viewport and no
+        // scene to keep the viewer alive.
+        osgViewer::View * pView = new osgViewer::View;
+        pView->getCamera()->setGraphicsContext( getGraphicsWindow() );
+        pView->getCamera()->setViewport( 0, 0, 0, 0 );
+        addView( pView );
 
-          // Clear the viewer of removed views
-          getGraphicsWindow()->setClearMask( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
-          getGraphicsWindow()->setClearColor( osg::Vec4( 0.08, 0.08, 0.5, 1.0 ) );
+        // Clear the viewer of removed views
+        getGraphicsWindow()->setClearMask( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
+        getGraphicsWindow()->setClearColor( osg::Vec4( 0.08, 0.08, 0.5, 1.0 ) );
 
-          // The app would hang on exit when using start(1).  Behaves better with 10
-          // like the non-composite viewer.  Was this just a typo?
-          _timer.start(1000.0f/fps);
-        }
+        // The app would hang on exit when using start(1).  Behaves better with 10
+        // like the non-composite viewer.  Was this just a typo?
+        _timer.start(1000.0f/fps);
+    }
 
-        virtual void paintEvent( QPaintEvent * event ) { frame(); }
+    virtual void paintEvent( QPaintEvent * event ) { frame(); }
 
-        void keyPressEvent( QKeyEvent* event )
+    void keyPressEvent( QKeyEvent* event )
+    {
+        if ( event->text() == "a" )
         {
-          if ( event->text() == "a" )
-          {
             AddView( _scene.get() );
-          }
+        }
 
-          if ( event->text() == "r" )
-          {
+        if ( event->text() == "r" )
+        {
             RemoveView();
-          }
-
-          QOSGWidget::keyPressEvent( event );
         }
 
+        QOSGWidget::keyPressEvent( event );
+    }
+
 
-        void AddView( osg::Node * scene );
-        void RemoveView();
-        void Tile();
+    void AddView( osg::Node * scene );
+    void RemoveView();
+    void Tile();
 
-        osg::ref_ptr< osg::Node > _scene;
+    osg::ref_ptr< osg::Node > _scene;
 
-    protected:
-        QTimer _timer;
+protected:
+    QTimer _timer;
 };
 
 void setupHandlers(osgViewer::View * viewer);

src/ui/map3D/Texture.cc

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of the class Texture.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
+#include <osg/LineWidth>
+
+#include "Texture.h"
+
+Texture::Texture()
+    : _is3D(false)
+{
+    texture2D->setFilter(osg::Texture::MIN_FILTER, osg::Texture::NEAREST);
+    texture2D->setFilter(osg::Texture::MAG_FILTER, osg::Texture::NEAREST);
+    GLuint id;
+    glGenTextures(1, &id);
+    t->setID(id);
+    glBindTexture(GL_TEXTURE_2D, id);
+    glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
+    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+}
+
+const QString&
+Texture::getSourceURL(void) const
+{
+    return sourceURL;
+}
+
+void
+Texture::setId(unsigned int _id)
+{
+    id = _id;
+}
+
+void
+Texture::sync(const WebImagePtr& image)
+{
+    state = static_cast<State>(image->getState());
+
+    if (image->getState() != WebImage::UNINITIALIZED &&
+        (sourceURL != image->getSourceURL() ||
+         _is3D != image->is3D()))
+    {
+        sourceURL = image->getSourceURL();
+        _is3D = image->is3D();
+    }
+
+    if (image->getState() == WebImage::READY && image->getSyncFlag())
+    {
+        image->setSyncFlag(false);
+
+        if (image->getWidth() != imageWidth ||
+            image->getHeight() != imageHeight)
+        {
+            imageWidth = image->getWidth();
+            textureWidth = 32;
+            while (textureWidth < imageWidth)
+            {
+                textureWidth *= 2;
+            }
+            imageHeight = image->getHeight();
+            textureHeight = 32;
+            while (textureHeight < imageHeight)
+            {
+                textureHeight *= 2;
+            }
+
+            maxU = static_cast<double>(imageWidth)
+                   / static_cast<double>(textureWidth);
+            maxV = static_cast<double>(imageHeight)
+                   / static_cast<double>(textureHeight);
+
+            osg::ref_ptr<osg::Image> image;
+            image->setImage(textureWidth, textureHeight, 8, 3, GL_RGBA, GL_UNSIGNED_BYTES, NULL, osg::Image::USE_NEW_DELETE);
+
+            texture2D->
+            glBindTexture(GL_TEXTURE_2D, id);
+            glTexImage2D(GL_TEXTURE_2D, 0, 3, textureWidth, textureHeight,
+                         0, GL_RGBA, GL_UNSIGNED_BYTE, NULL);
+        }
+
+        glBindTexture(GL_TEXTURE_2D, id);
+        glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, imageWidth, imageHeight,
+                        GL_RGBA, GL_UNSIGNED_BYTE, image->getImageData());
+
+        heightModel = image->getHeightModel();
+    }
+}
+
+osg::ref_ptr<osg::Geometry>
+Texture::draw(float x1, float y1, float x2, float y2,
+              bool smoothInterpolation) const
+{
+    return draw(x1, y1, x2, y1, x2, y2, x1, y2, smoothInterpolation);
+}
+
+osg::ref_ptr<osg::Geometry>
+Texture::draw(float x1, float y1, float x2, float y2,
+              float x3, float y3, float x4, float y4,
+              bool smoothInterpolation) const
+{
+    osg::ref_ptr<osg::Geometry> geometry(new osg::Geometry);
+    osg::ref_ptr<osg::StateSet> stateset(new osg::StateSet);
+
+    if (state == REQUESTED)
+    {
+        osg::ref_ptr<osg::Vec2Array> vertices(new osg::Vec2Array);
+        vertices->push_back(osg::Vec2(x1, y1));
+        vertices->push_back(osg::Vec2(x2, y2));
+        vertices->push_back(osg::Vec2(x3, y3));
+        vertices->push_back(osg::Vec2(x4, y4));
+
+        geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES,
+                                                      0, vertices->size()));
+
+        geometry->setVertexArray(vertices);
+
+        osg::ref_ptr<osg::Vec4Array> color(new osg::Vec4Array);
+        color->push_back(osg::Vec4(0.0f, 0.0f, 1.0f, 1.0f));
+        geometry->setColorArray(color);
+        geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
+        
+        return geometry;
+    }
+
+    stateset->setTextureAttributeAndModes(id, texture2D);
+
+    float dx, dy;
+    if (smoothInterpolation)
+    {
+        texture2D->setFilter(osg::Texture::MIN_FILTER, osg::Texture::LINEAR);
+        texture2D->setFilter(osg::Texture::MAG_FILTER, osg::Texture::LINEAR);
+        dx = 1.0f / (2.0f * textureWidth);
+        dy = 1.0f / (2.0f * textureHeight);
+    }
+    else
+    {
+        texture2D->setFilter(osg::Texture::MIN_FILTER, osg::Texture::NEAREST);
+        texture2D->setFilter(osg::Texture::MAG_FILTER, osg::Texture::NEAREST);
+        dx = 0.0f;
+        dy = 0.0f;
+    }
+
+    glColor3f(1.0f, 1.0f, 1.0f);
+    if (!_is3D)
+    {
+        osg::ref_ptr<osg::Vec2Array> tc = new osg::Vec2Array;
+
+        geometry->setTexCoordArray(id, tc);
+        tc->push_back(osg::Vec2(dx, maxV - dy));
+        tc->push_back(osg::Vec2(maxU - dx, maxV - dy));
+        tc->push_back(osg::Vec2(maxU - dx, dy));
+        tc->push_back(osg::Vec2(dx, dy));
+
+        glBegin(GL_QUADS);
+        glTexCoord2f(dx, maxV - dy);
+        glVertex3f(x1, y1, 0.0f);
+        glTexCoord2f(maxU - dx, maxV - dy);
+        glVertex3f(x2, y2, 0.0f);
+        glTexCoord2f(maxU - dx, dy);
+        glVertex3f(x3, y3, 0.0f);
+        glTexCoord2f(dx, dy);
+        glVertex3f(x4, y4, 0.0f);
+        glEnd();
+    }
+    else
+    {
+        float scaleX = 1.0f / static_cast<float>(heightModel.size() - 1);
+
+        for (int32_t i = 0; i < heightModel.size() - 1; ++i)
+        {
+            float scaleI = scaleX * static_cast<float>(i);
+
+            float scaleY =
+                    1.0f / static_cast<float>(heightModel[i].size() - 1);
+
+            float x1i = x1 + scaleI * (x4 - x1);
+            float x1f = x2 + scaleI * (x3 - x2);
+            float x2i = x1i + scaleX * (x4 - x1);
+            float x2f = x1f + scaleX * (x3 - x2);
+
+            for (int32_t j = 0; j < heightModel[i].size() - 1; ++j)
+            {
+                float scaleJ = scaleY * static_cast<float>(j);
+
+                float y1i = y1 + scaleJ * (y2 - y1);
+                float y1f = y4 + scaleJ * (y3 - y4);
+                float y2i = y1i + scaleY * (y2 - y1);
+                float y2f = y1f + scaleY * (y3 - y4);
+
+                float nx1 = x1i + scaleJ * (x1f - x1i);
+                float nx2 = x1i + (scaleJ + scaleY) * (x1f - x1i);
+                float nx3 = x2i + (scaleJ + scaleY) * (x2f - x2i);
+                float nx4  = x2i + scaleJ * (x2f - x2i);
+                float ny1 = y1i + scaleI * (y1f - y1i);
+                float ny2 = y2i + scaleI * (y2f - y2i);
+                float ny3 = y2i + (scaleI + scaleX) * (y2f - y2i);
+                float ny4 = y1i + (scaleI + scaleX) * (y1f - y1i);
+
+                glBegin(GL_QUADS);
+                glTexCoord2f(dx + scaleJ * (maxU - dx * 2.0f),
+                             dy + (1.0f - scaleI) * (maxV - dy * 2.0f));
+                glVertex3f(nx1, ny1, -static_cast<float>(heightModel[i][j]));
+                glTexCoord2f(dx + (scaleJ + scaleY) * (maxU - dx * 2.0f),
+                             dy + (1.0f - scaleI) * (maxV - dy * 2.0f));
+                glVertex3f(nx2, ny2, -static_cast<float>(heightModel[i][j + 1]));
+                glTexCoord2f(dx + (scaleJ + scaleY) * (maxU - dx * 2.0f),
+                             dy + (1.0f - scaleI - scaleX) * (maxV - dy * 2.0f));
+                glVertex3f(nx3, ny3, -static_cast<float>(heightModel[i + 1][j + 1]));
+                glTexCoord2f(dx + scaleJ * (maxU - dx * 2.0f),
+                             dy + (1.0f - scaleI - scaleX) * (maxV - dy * 2.0f));
+                glVertex3f(nx4, ny4, -static_cast<float>(heightModel[i + 1][j]));
+
+                glEnd();
+            }
+        }
+    }
+}
+
+bool
+Texture::is3D(void) const
+{
+    return _is3D;
+}

src/ui/map3D/Texture.h

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of the class Texture.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
+#ifndef TEXTURE_H
+#define TEXTURE_H
+
+#if (defined __APPLE__) & (defined __MACH__)
+#include <OpenGL/gl.h>
+#else
+#include <GL/gl.h>
+#endif
+#include <inttypes.h>
+#include <osg/ref_ptr>
+#include <osg/Geometry>
+#include <QSharedPointer>
+
+#include "WebImage.h"
+
+class Texture
+{
+public:
+    Texture();
+
+    const QString& getSourceURL(void) const;
+
+    void setId(unsigned int _id);
+
+    void sync(const WebImagePtr& image);
+
+    osg::ref_ptr<osg::Geometry> draw(float x1, float y1, float x2, float y2,
+                                     bool smoothInterpolation) const;
+    osg::ref_ptr<osg::Geometry> draw(float x1, float y1, float x2, float y2,
+                                     float x3, float y3, float x4, float y4,
+                                     bool smoothInterpolation) const;
+
+    bool is3D(void) const;
+
+private:
+    enum State
+    {
+        UNINITIALIZED = 0,
+        REQUESTED = 1,
+        READY = 2
+    };
+
+    State state;
+    QString sourceURL;
+    unsigned int id;
+    osg::ref_ptr<osg::Texture2D> texture2D;
+
+    int32_t textureWidth;
+    int32_t textureHeight;
+
+    int32_t imageWidth;
+    int32_t imageHeight;
+
+    bool _is3D;
+    QVector< QVector<int32_t> > heightModel;
+
+    float maxU;
+    float maxV;
+};
+
+typedef QSharedPointer<Texture> TexturePtr;
+
+#endif // TEXTURE_H

src/ui/map3D/TextureCache.cc

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of the class TextureCache.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
+#include "TextureCache.h"
+
+TextureCache::TextureCache(uint32_t _cacheSize)
+    : cacheSize(_cacheSize)
+    , imageCache(new WebImageCache(0, cacheSize))
+{
+    for (uint32_t i = 0; i < cacheSize; ++i)
+    {
+        TexturePtr t(new Texture);
+        t->setId(i);
+
+        textures.push_back(t);
+    }
+}
+
+TexturePtr
+TextureCache::get(const QString& tileURL, bool useHeightModel)
+{
+    QPair<TexturePtr, int32_t> p1 = lookup(tileURL, useHeightModel);
+    if (!p1.first.isNull())
+    {
+        return p1.first;
+    }
+
+    QPair<WebImagePtr, int32_t> p2 =
+            imageCache->lookup(tileURL, useHeightModel);
+    if (!p2.first.isNull())
+    {
+        textures[p2.second]->sync(p2.first);
+        p1 = lookup(tileURL, useHeightModel);
+
+        return p1.first;
+    }
+
+    return TexturePtr();
+}
+
+void
+TextureCache::sync(void)
+{
+    if (requireSync())
+    {
+        for (int32_t i = 0; i < textures.size(); ++i)
+        {
+            textures[i]->sync(imageCache->at(i));
+        }
+    }
+}
+
+QPair<TexturePtr, int32_t>
+TextureCache::lookup(const QString& tileURL, bool useHeightModel)
+{
+    for (int32_t i = 0; i < textures.size(); ++i)
+    {
+        if (textures[i]->getSourceURL() == tileURL &&
+            textures[i]->is3D() == useHeightModel)
+        {
+            return qMakePair(textures[i], i);
+        }
+    }
+
+    return qMakePair(TexturePtr(), -1);
+}
+
+bool
+TextureCache::requireSync(void) const
+{
+    for (uint32_t i = 0; i < cacheSize; ++i)
+    {
+        if (imageCache->at(i)->getSyncFlag())
+        {
+            return true;
+        }
+    }
+    return false;
+}

src/ui/map3D/TextureCache.h

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of the class TextureCache.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
+#ifndef TEXTURECACHE_H
+#define TEXTURECACHE_H
+
+#include <QVector>
+
+#include "Texture.h"
+#include "WebImageCache.h"
+
+class TextureCache
+{
+public:
+    explicit TextureCache(uint32_t cacheSize);
+
+    TexturePtr get(const QString& tileURL, bool useHeightModel = false);
+
+    void sync(void);
+
+private:
+    QPair<TexturePtr, int32_t> lookup(const QString& tileURL,
+                                      bool useHeightModel);
+
+    bool requireSync(void) const;
+
+    uint32_t cacheSize;
+    QVector<TexturePtr> textures;
+
+    QScopedPointer<WebImageCache> imageCache;
+};
+
+#endif // TEXTURECACHE_H

src/ui/map3D/WebImage.cc

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of the class WebImage.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
+#include "WebImage.h"
+
+#include <QFile>
+#include <QGLWidget>
+
+WebImage::WebImage()
+    : state(WebImage::UNINITIALIZED)
+    , sourceURL("")
+    , image(0)
+    , lastReference(0)
+    , _is3D(false)
+    , syncFlag(false)
+{
+
+}
+
+void
+WebImage::clear(void)
+{
+    image.reset();
+    sourceURL.clear();
+    state = WebImage::UNINITIALIZED;
+    lastReference = 0;
+    heightModel.clear();
+}
+
+WebImage::State
+WebImage::getState(void) const
+{
+    return state;
+}
+
+void
+WebImage::setState(State state)
+{
+    this->state = state;
+}
+
+const QString&
+WebImage::getSourceURL(void) const
+{
+    return sourceURL;
+}
+
+void
+WebImage::setSourceURL(const QString& url)
+{
+    sourceURL = url;
+}
+
+const uint8_t*
+WebImage::getImageData(void) const
+{
+    return image->scanLine(0);
+}
+
+const QVector< QVector<int32_t> >&
+WebImage::getHeightModel(void) const
+{
+    return heightModel;
+}
+
+bool
+WebImage::setData(const QByteArray& data)
+{
+    QImage tempImage;
+    if (tempImage.loadFromData(data))
+    {
+        if (image.isNull())
+        {
+            image.reset(new QImage);
+        }
+        *image = QGLWidget::convertToGLFormat(tempImage);
+
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+bool
+WebImage::setData(const QString& filename)
+{
+    QImage tempImage;
+    if (tempImage.load(filename))
+    {
+        if (image.isNull())
+        {
+            image.reset(new QImage);
+        }
+        *image = QGLWidget::convertToGLFormat(tempImage);
+
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+bool
+WebImage::setData(const QString& imageFilename, const QString& heightFilename)
+{
+    QFile heightFile(heightFilename);
+
+    QImage tempImage;
+    if (tempImage.load(imageFilename) && heightFile.open(QIODevice::ReadOnly))
+    {
+        if (image.isNull())
+        {
+            image.reset(new QImage);
+        }
+        *image = QGLWidget::convertToGLFormat(tempImage);
+
+        QDataStream heightDataStream(&heightFile);
+
+        // read in width and height values for height map
+        char header[8];
+        heightDataStream.readRawData(header, 8);
+
+        int32_t height = *(reinterpret_cast<int32_t *>(header));
+        int32_t width = *(reinterpret_cast<int32_t *>(header + 4));
+
+        char buffer[height * width * sizeof(int32_t)];
+        heightDataStream.readRawData(buffer, height * width * sizeof(int32_t));
+
+        heightModel.clear();
+        for (int32_t i = 0; i < height; ++i)
+        {
+            QVector<int32_t> scanline;
+            for (int32_t j = 0; j < width; ++j)
+            {    
+                int32_t n = *(reinterpret_cast<int32_t *>(buffer
+                                                          + (i * height + j)
+                                                          * sizeof(int32_t)));
+                scanline.push_back(n);
+            }
+            heightModel.push_back(scanline);
+        }
+
+        heightFile.close();
+
+        _is3D = true;
+
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+int32_t
+WebImage::getWidth(void) const
+{
+    return image->width();
+}
+
+int32_t
+WebImage::getHeight(void) const
+{
+    return image->height();
+}
+
+int32_t
+WebImage::getByteCount(void) const
+{
+    return image->byteCount();
+}
+
+bool
+WebImage::is3D(void) const
+{
+    return _is3D;
+}
+
+uint64_t
+WebImage::getLastReference(void) const
+{
+    return lastReference;
+}
+
+void
+WebImage::setLastReference(uint64_t value)
+{
+    lastReference = value;
+}
+
+bool
+WebImage::getSyncFlag(void) const
+{
+    return syncFlag;
+}
+
+void
+WebImage::setSyncFlag(bool onoff)
+{
+    syncFlag = onoff;
+}

src/ui/map3D/WebImage.h

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of the class WebImage.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
+#ifndef WEBIMAGE_H
+#define WEBIMAGE_H
+
+#include <inttypes.h>
+#include <QImage>
+#include <QScopedPointer>
+#include <QSharedPointer>
+
+class WebImage
+{
+public:
+    WebImage();
+
+    void clear(void);
+
+    enum State
+    {
+        UNINITIALIZED = 0,
+        REQUESTED = 1,
+        READY = 2
+    };
+
+    State getState(void) const;
+    void setState(State state);
+
+    const QString& getSourceURL(void) const;
+    void setSourceURL(const QString& url);
+
+    const uint8_t* getImageData(void) const;
+    const QVector< QVector<int32_t> >& getHeightModel(void) const;
+    bool setData(const QByteArray& data);
+    bool setData(const QString& filename);
+    bool setData(const QString& imageFilename, const QString& heightFilename);
+
+    int32_t getWidth(void) const;
+    int32_t getHeight(void) const;
+    int32_t getByteCount(void) const;
+
+    bool is3D(void) const;
+
+    uint64_t getLastReference(void) const;
+    void setLastReference(uint64_t value);
+
+    bool getSyncFlag(void) const;
+    void setSyncFlag(bool onoff);
+
+private:
+    State state;
+    QString sourceURL;
+    QScopedPointer<QImage> image;
+    QVector< QVector<int32_t> > heightModel;
+    uint64_t lastReference;
+    bool _is3D;
+    bool syncFlag;
+};
+
+typedef QSharedPointer<WebImage> WebImagePtr;
+
+#endif // WEBIMAGE_H

src/ui/map3D/WebImageCache.cc

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of the class WebImageCache.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
+#include "WebImageCache.h"
+
+#include <QNetworkReply>
+#include <QPixmap>
+
+WebImageCache::WebImageCache(QObject* parent, uint32_t _cacheSize)
+    : QObject(parent)
+    , cacheSize(_cacheSize)
+    , currentReference(0)
+    , networkManager(new QNetworkAccessManager)
+{
+    for (uint32_t i = 0; i < cacheSize; ++i)
+    {
+        WebImagePtr image(new WebImage);
+
+        webImages.push_back(image);
+    }
+
+    connect(networkManager.data(), SIGNAL(finished(QNetworkReply*)),
+            this, SLOT(downloadFinished(QNetworkReply*)));
+}
+
+QPair<WebImagePtr, int32_t>
+WebImageCache::lookup(const QString& url, bool useHeightModel)
+{
+    QPair<WebImagePtr, int32_t> cacheEntry;
+
+    for (int32_t i = 0; i < webImages.size(); ++i)
+    {
+        if (webImages[i]->getState() != WebImage::UNINITIALIZED &&
+            webImages[i]->getSourceURL() == url &&
+            webImages[i]->is3D() == useHeightModel)
+        {
+            cacheEntry.first = webImages[i];
+            cacheEntry.second = i;
+            break;
+        }
+    }
+
+    if (cacheEntry.first.isNull())
+    {
+        for (int32_t i = 0; i < webImages.size(); ++i)
+        {
+            // get uninitialized image
+            if (webImages[i]->getState() == WebImage::UNINITIALIZED)
+            {
+                cacheEntry.first = webImages[i];
+                cacheEntry.second = i;
+                break;
+            }
+            // get oldest image
+            else if (webImages[i]->getState() == WebImage::READY &&
+                     (cacheEntry.first.isNull() ||
+                      webImages[i]->getLastReference() <
+                      cacheEntry.first->getLastReference()))
+            {
+                cacheEntry.first = webImages[i];
+                cacheEntry.second = i;
+            }
+        }
+
+        if (cacheEntry.first.isNull())
+        {
+            return qMakePair(WebImagePtr(), -1);
+        }
+        else
+        {
+            if (cacheEntry.first->getState() == WebImage::READY)
+            {
+                cacheEntry.first->clear();
+            }
+            cacheEntry.first->setSourceURL(url);
+            cacheEntry.first->setLastReference(currentReference);
+            ++currentReference;
+            cacheEntry.first->setState(WebImage::REQUESTED);
+
+            if (url.left(4).compare("http") == 0)
+            {
+                networkManager->get(QNetworkRequest(QUrl(url)));
+            }
+            else
+            {
+                bool success;
+
+                if (useHeightModel)
+                {
+                    QString heightURL = url;
+                    heightURL.replace("color", "dom");
+                    heightURL.replace(".jpg", ".txt");
+
+                    success = cacheEntry.first->setData(url, heightURL);
+                }
+                else
+                {
+                    success = cacheEntry.first->setData(url);
+                }
+
+                if (success)
+                {
+                    cacheEntry.first->setSyncFlag(true);
+                    cacheEntry.first->setState(WebImage::READY);
+                }
+                else
+                {
+                    cacheEntry.first->setState(WebImage::UNINITIALIZED);
+                }
+            }
+
+            return cacheEntry;
+        }
+    }
+    else
+    {
+        if (cacheEntry.first->getState() == WebImage::READY)
+        {
+            cacheEntry.first->setLastReference(currentReference);
+            ++currentReference;
+            return cacheEntry;
+        }
+        else
+        {
+            return qMakePair(WebImagePtr(), -1);
+        }
+    }
+}
+
+WebImagePtr
+WebImageCache::at(int32_t index) const
+{
+    return webImages[index];
+}
+
+void
+WebImageCache::downloadFinished(QNetworkReply* reply)
+{
+    reply->deleteLater();
+
+    if (reply->error() != QNetworkReply::NoError)
+    {
+        return;
+    }
+    QVariant attribute = reply->attribute(QNetworkRequest::RedirectionTargetAttribute);
+    if (attribute.isValid())
+    {
+        return;
+    }
+
+    WebImagePtr image;
+    foreach(image, webImages)
+    {
+        if (reply->url().toString() == image->getSourceURL())
+        {
+            image->setData(reply->readAll());
+            image->setSyncFlag(true);
+            image->setState(WebImage::READY);
+
+            return;
+        }
+    }
+}

src/ui/map3D/WebImageCache.h

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of the class WebImageCache.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
+#ifndef WEBIMAGECACHE_H
+#define WEBIMAGECACHE_H
+
+#include <QNetworkAccessManager>
+#include <QObject>
+#include <QPair>
+
+#include "WebImage.h"
+
+class WebImageCache : public QObject
+{
+    Q_OBJECT
+
+public:
+    WebImageCache(QObject* parent, uint32_t cacheSize);
+
+    QPair<WebImagePtr, int32_t> lookup(const QString& url,
+                                       bool useHeightModel);
+
+    WebImagePtr at(int32_t index) const;
+
+private Q_SLOTS:
+    void downloadFinished(QNetworkReply* reply);
+
+private:
+    uint32_t cacheSize;
+
+    QVector<WebImagePtr> webImages;
+    uint64_t currentReference;
+
+    QScopedPointer<QNetworkAccessManager> networkManager;
+};
+
+#endif // WEBIMAGECACHE_H