Merge branch 'master' of pixhawk.ethz.ch:qgroundcontrol into dev

src/Core.cc

@@ -114,6 +114,9 @@ Core::Core(int &argc, char* argv[]) : QApplication(argc, argv)
     // to make sure that all components are initialized when the
     // first messages arrive
     UDPLink* udpLink = new UDPLink(QHostAddress::Any, 14550);
+    // Listen on Multicast-Address 239.255.77.77, Port 14550
+    //QHostAddress * multicast_udp = new QHostAddress("239.255.77.77");
+    //UDPLink* udpLink = new UDPLink(*multicast_udp, 14550);
     mainWindow->addLink(udpLink);
 
     // Check if link could be connected

src/comm/UDPLink.cc

@@ -36,6 +36,7 @@ This file is part of the QGROUNDCONTROL project
 #include "UDPLink.h"
 #include "LinkManager.h"
 #include "MG.h"
+#include <netinet/in.h>
 
 UDPLink::UDPLink(QHostAddress host, quint16 port)
 {
@@ -197,11 +198,46 @@ bool UDPLink::connect()
 {
     socket = new QUdpSocket(this);
 
+    //Check if we are using a multicast-address
+//    bool multicast = false;
+//    if (host.isInSubnet(QHostAddress("224.0.0.0"),4))
+//    {
+//        multicast = true;
+//        connectState = socket->bind(port, QUdpSocket::ShareAddress);
+//    }
+//    else
+//    {
+        connectState = socket->bind(host, port);
+//    }
+
+    //Provides Multicast functionality to UdpSocket
+    /* not working yet
+    if (multicast)
+    {
+        int sendingFd = socket->socketDescriptor();
+
+        if (sendingFd != -1)
+        {
+            // set up destination address
+            struct sockaddr_in sendAddr;
+            memset(&sendAddr,0,sizeof(sendAddr));
+            sendAddr.sin_family=AF_INET;
+            sendAddr.sin_addr.s_addr=inet_addr(HELLO_GROUP);
+            sendAddr.sin_port=htons(port);
+
+            // set TTL
+            unsigned int ttl = 1; // restricted to the same subnet
+            if (setsockopt(sendingFd, IPPROTO_IP, IP_MULTICAST_TTL, (unsigned int*)&ttl, sizeof(ttl) ) < 0)
+            {
+                std::cout << "TTL failed\n";
+            }
+        }
+    }
+    */
+
     //QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readPendingDatagrams()));
     QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes()));
 
-    connectState = socket->bind(host, port);
-
     emit connected(connectState);
     if (connectState)
     {

src/comm/UDPLink.h

@@ -46,6 +46,7 @@ class UDPLink : public LinkInterface
 
 public:
     UDPLink(QHostAddress host = QHostAddress::Any, quint16 port = 14550);
+    //UDPLink(QHostAddress host = "239.255.76.67", quint16 port = 7667);
     ~UDPLink();
 
     bool isConnected();

src/ui/map3D/Imagery.cc

@@ -32,6 +32,7 @@ This file is part of the QGROUNDCONTROL project
 #include "Imagery.h"
 
 #include <cmath>
+#include <iomanip>
 #include <sstream>
 
 const double WGS84_A = 6378137.0;
@@ -65,7 +66,8 @@ Imagery::prefetch2D(double windowWidth, double windowHeight,
                     const QString& utmZone)
 {
     double tileResolution;
-    if (currentImageryType == SATELLITE)
+    if (currentImageryType == GOOGLE_SATELLITE ||
+        currentImageryType == GOOGLE_MAP)
     {
         tileResolution = 1.0;
         while (tileResolution * 3.0 / 2.0 < 1.0 / zoom)
@@ -77,6 +79,10 @@ Imagery::prefetch2D(double windowWidth, double windowHeight,
             tileResolution = 512.0;
         }
     }
+    else if (currentImageryType == SWISSTOPO_SATELLITE)
+    {
+        tileResolution = 0.25;
+    }
 
     int32_t minTileX, minTileY, maxTileX, maxTileY;
     int32_t zoomLevel;
@@ -92,7 +98,7 @@ Imagery::prefetch2D(double windowWidth, double windowHeight,
     {
         for (int32_t c = minTileX; c <= maxTileX; ++c)
         {
-            QString url = getTileURL(c, r, zoomLevel);
+            QString url = getTileLocation(c, r, zoomLevel, tileResolution);
 
             TexturePtr t = textureCache->get(url);
         }
@@ -106,7 +112,8 @@ Imagery::draw2D(double windowWidth, double windowHeight,
                 const QString& utmZone)
 {
     double tileResolution;
-    if (currentImageryType == SATELLITE)
+    if (currentImageryType == GOOGLE_SATELLITE ||
+        currentImageryType == GOOGLE_MAP)
     {
         tileResolution = 1.0;
         while (tileResolution * 3.0 / 2.0 < 1.0 / zoom)
@@ -118,6 +125,10 @@ Imagery::draw2D(double windowWidth, double windowHeight,
             tileResolution = 512.0;
         }
     }
+    else if (currentImageryType == SWISSTOPO_SATELLITE)
+    {
+        tileResolution = 0.25;
+    }
 
     int32_t minTileX, minTileY, maxTileX, maxTileY;
     int32_t zoomLevel;
@@ -133,7 +144,7 @@ Imagery::draw2D(double windowWidth, double windowHeight,
     {
         for (int32_t c = minTileX; c <= maxTileX; ++c)
         {
-            QString tileURL = getTileURL(c, r, zoomLevel);
+            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);
@@ -154,7 +165,7 @@ void
 Imagery::prefetch3D(double radius, double tileResolution,
                     double xOrigin, double yOrigin,
                     double viewXOffset, double viewYOffset,
-                    const QString& utmZone)
+                    const QString& utmZone, bool useHeightModel)
 {
     int32_t minTileX, minTileY, maxTileX, maxTileY;
     int32_t zoomLevel;
@@ -170,9 +181,9 @@ Imagery::prefetch3D(double radius, double tileResolution,
     {
         for (int32_t c = minTileX; c <= maxTileX; ++c)
         {
-            QString url = getTileURL(c, r, zoomLevel);
+            QString url = getTileLocation(c, r, zoomLevel, tileResolution);
 
-            TexturePtr t = textureCache->get(url);
+            TexturePtr t = textureCache->get(url, useHeightModel);
         }
     }
 }
@@ -181,7 +192,7 @@ void
 Imagery::draw3D(double radius, double tileResolution,
                 double xOrigin, double yOrigin,
                 double viewXOffset, double viewYOffset,
-                const QString& utmZone)
+                const QString& utmZone, bool useHeightModel)
 {
     int32_t minTileX, minTileY, maxTileX, maxTileY;
     int32_t zoomLevel;
@@ -197,12 +208,12 @@ Imagery::draw3D(double radius, double tileResolution,
     {
         for (int32_t c = minTileX; c <= maxTileX; ++c)
         {
-            QString tileURL = getTileURL(c, r, zoomLevel);
+            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);
+            TexturePtr t = textureCache->get(tileURL, useHeightModel);
 
             if (!t.isNull())
             {
@@ -228,6 +239,9 @@ Imagery::imageBounds(int32_t tileX, int32_t 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)
+    {
         int32_t zoomLevel = MAX_ZOOM_LEVEL - static_cast<int32_t>(rint(log2(tileResolution)));
         int32_t numTiles = static_cast<int32_t>(exp2(static_cast<double>(zoomLevel)));
 
@@ -242,6 +256,21 @@ Imagery::imageBounds(int32_t tileX, int32_t tileY, double tileResolution,
         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
@@ -254,15 +283,30 @@ Imagery::tileBounds(double tileResolution,
 {
     double centerUtmX = (maxUtmX - minUtmX) / 2.0 + minUtmX;
     double centerUtmY = (maxUtmY - minUtmY) / 2.0 + minUtmY;
-
     int32_t 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<int32_t>(rint(minUtmX / utmMultiplier));
+        minTileY = static_cast<int32_t>(rint(minUtmY / utmMultiplier));
+        centerTileX = static_cast<int32_t>(rint(centerUtmX / utmMultiplier));
+        centerTileY = static_cast<int32_t>(rint(centerUtmY / utmMultiplier));
+        maxTileX = static_cast<int32_t>(rint(maxUtmX / utmMultiplier));
+        maxTileY = static_cast<int32_t>(rint(maxUtmY / utmMultiplier));
+    }
 
     if (maxTileX - minTileX + 1 > 14)
     {
@@ -513,20 +557,33 @@ Imagery::UTMtoLL(double utmNorthing, double utmEasting, const QString& utmZone,
 }
 
 QString
-Imagery::getTileURL(int32_t tileX, int32_t tileY, int32_t zoomLevel) const
+Imagery::getTileLocation(int32_t tileX, int32_t tileY, int32_t zoomLevel,
+                         double tileResolution) const
 {
     std::ostringstream oss;
 
     switch (currentImageryType)
     {
-    case MAP:
+    case GOOGLE_MAP:
         oss << "http://mt0.google.com/vt/lyrs=m@120&x=" << tileX
             << "&y=" << tileY << "&z=" << zoomLevel;
         break;
-    case SATELLITE:
+    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<int32_t>(rint(tileResolution));
+        }
+        oss << "-" << tileY << "-" << tileX << ".jpg";
     default:
         {};
     }

src/ui/map3D/Imagery.h

@@ -43,8 +43,10 @@ public:
 
     enum ImageryType
     {
-        MAP = 0,
-        SATELLITE = 1
+        GOOGLE_MAP = 0,
+        GOOGLE_SATELLITE = 1,
+        SWISSTOPO_SATELLITE = 2,
+        SWISSTOPO_SATELLITE_3D = 3
     };
 
     void setImageryType(ImageryType type);
@@ -62,11 +64,11 @@ public:
     void prefetch3D(double radius, double tileResolution,
                     double xOrigin, double yOrigin,
                     double viewXOffset, double viewYOffset,
-                    const QString& utmZone);
+                    const QString& utmZone, bool useHeightModel);
     void draw3D(double radius, double tileResolution,
                 double xOrigin, double yOrigin,
                 double viewXOffset, double viewYOffset,
-                const QString& utmZone);
+                const QString& utmZone, bool useHeightModel);
 
     bool update(void);
 
@@ -100,7 +102,8 @@ private:
     void UTMtoLL(double utmNorthing, double utmEasting, const QString& utmZone,
                  double& latitude, double& longitude) const;
 
-    QString getTileURL(int32_t tileX, int32_t tileY, int32_t zoomLevel) const;
+    QString getTileLocation(int32_t tileX, int32_t tileY, int32_t zoomLevel,
+                            double tileResolution) const;
 
     ImageryType currentImageryType;
 

src/ui/map3D/Q3DWidget.cc

@@ -356,7 +356,6 @@ Q3DWidget::getMouseY(void)
     return mapFromGlobal(cursor().pos()).y();
 }
 
-
 void
 Q3DWidget::rotateCamera(float dx, float dy)
 {
@@ -858,9 +857,12 @@ Q3DWidget::timerEvent(QTimerEvent* event)
 }
 
 void
-Q3DWidget::closeEvent(QCloseEvent *)
+Q3DWidget::closeEvent(QCloseEvent* event)
 {
     // exit application
+    timer.stop();
+
+    event->accept();
 }
 
 void

src/ui/map3D/QMap3DWidget.cc

@@ -95,6 +95,8 @@ QMap3DWidget::buildLayout(void)
     imageryComboBox->addItem("None");
     imageryComboBox->addItem("Map (Google)");
     imageryComboBox->addItem("Satellite (Google)");
+    imageryComboBox->addItem("Satellite (Swisstopo)");
+    imageryComboBox->addItem("3D Satellite (Swisstopo)");
 
     QPushButton* recenterButton = new QPushButton(this);
     recenterButton->setText("Recenter Camera");
@@ -536,11 +538,19 @@ QMap3DWidget::showImagery(const QString& text)
     {
         if (text.compare("Map (Google)") == 0)
         {
-            imagery->setImageryType(Imagery::MAP);
+            imagery->setImageryType(Imagery::GOOGLE_MAP);
         }
         else if (text.compare("Satellite (Google)") == 0)
         {
-            imagery->setImageryType(Imagery::SATELLITE);
+            imagery->setImageryType(Imagery::GOOGLE_SATELLITE);
+        }
+        else if (text.compare("Satellite (Swisstopo)") == 0)
+        {
+            imagery->setImageryType(Imagery::SWISSTOPO_SATELLITE);
+        }
+        else if (text.compare("3D Satellite (Swisstopo)") == 0)
+        {
+            imagery->setImageryType(Imagery::SWISSTOPO_SATELLITE_3D);
         }
         displayImagery = true;
     }
@@ -673,6 +683,7 @@ QMap3DWidget::drawImagery(double originX, double originY, double originZ,
     double minResolution = 0.25;
     double centerResolution = cameraPose.distance / 100.0;
     double maxResolution = 1048576.0;
+    bool useHeightModel = false;
 
     if (imageryComboBox->currentText().compare("Map (Google)") == 0)
     {
@@ -682,6 +693,17 @@ QMap3DWidget::drawImagery(double originX, double originY, double originZ,
     {
         minResolution = 0.5;
     }
+    else if (imageryComboBox->currentText().compare("Satellite (Swisstopo)") == 0)
+    {
+        minResolution = 0.25;
+        maxResolution = 0.25;
+    }
+    else if (imageryComboBox->currentText().compare("3D Satellite (Swisstopo)") == 0)
+    {
+        minResolution = 0.25;
+        maxResolution = 0.25;
+        useHeightModel = true;
+    }
 
     double resolution = minResolution;
     while (resolution * 2.0 < centerResolution)
@@ -694,7 +716,8 @@ QMap3DWidget::drawImagery(double originX, double originY, double originZ,
     }
 
     imagery->draw3D(viewingRadius, resolution, originX, originY,
-                    cameraPose.xOffset, cameraPose.yOffset, zone);
+                    cameraPose.xOffset, cameraPose.yOffset, zone,
+                    useHeightModel);
 
     if (prefetch)
     {
@@ -702,13 +725,15 @@ QMap3DWidget::drawImagery(double originX, double originY, double originZ,
         {
             imagery->prefetch3D(viewingRadius / 2.0, resolution / 2.0,
                                 originX, originY,
-                                cameraPose.xOffset, cameraPose.yOffset, zone);
+                                cameraPose.xOffset, cameraPose.yOffset, zone,
+                                useHeightModel);
         }
         if (resolution * 2.0 <= maxResolution)
         {
             imagery->prefetch3D(viewingRadius * 2.0, resolution * 2.0,
                                 originX, originY,
-                                cameraPose.xOffset, cameraPose.yOffset, zone);
+                                cameraPose.xOffset, cameraPose.yOffset, zone,
+                                useHeightModel);
         }
     }
 

src/ui/map3D/Texture.cc

@@ -32,6 +32,7 @@ This file is part of the QGROUNDCONTROL project
 #include "Texture.h"
 
 Texture::Texture()
+    : _is3D(false)
 {
 
 }
@@ -54,9 +55,11 @@ Texture::sync(const WebImagePtr& image)
     state = static_cast<State>(image->getState());
 
     if (image->getState() != WebImage::UNINITIALIZED &&
-        sourceURL != image->getSourceURL())
+        (sourceURL != image->getSourceURL() ||
+         _is3D != image->is3D()))
     {
         sourceURL = image->getSourceURL();
+        _is3D = image->is3D();
     }
 
     if (image->getState() == WebImage::READY && image->getSyncFlag())
@@ -91,7 +94,9 @@ Texture::sync(const WebImagePtr& image)
 
         glBindTexture(GL_TEXTURE_2D, id);
         glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, imageWidth, imageHeight,
-                        GL_RGBA, GL_UNSIGNED_BYTE, image->getData());
+                        GL_RGBA, GL_UNSIGNED_BYTE, image->getImageData());
+
+        heightModel = image->getHeightModel();
     }
 }
 
@@ -140,18 +145,77 @@ Texture::draw(float x1, float y1, float x2, float y2,
     }
 
     glColor3f(1.0f, 1.0f, 1.0f);
+    if (!_is3D)
+    {
         glBegin(GL_QUADS);
-
         glTexCoord2f(dx, maxV - dy);
-    glVertex2f(x1, y1);
+        glVertex3f(x1, y1, 0.0f);
         glTexCoord2f(maxU - dx, maxV - dy);
-    glVertex2f(x2, y2);
+        glVertex3f(x2, y2, 0.0f);
         glTexCoord2f(maxU - dx, dy);
-    glVertex2f(x3, y3);
+        glVertex3f(x3, y3, 0.0f);
         glTexCoord2f(dx, dy);
-    glVertex2f(x4, y4);
+        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();
+            }
+        }
+    }
 
     glDisable(GL_TEXTURE_2D);
 }
+
+bool
+Texture::is3D(void) const
+{
+    return _is3D;
+}

src/ui/map3D/Texture.h

@@ -59,6 +59,8 @@ public:
               float x3, float y3, float x4, float y4,
               bool smoothInterpolation) const;
 
+    bool is3D(void) const;
+
 private:
     enum State
     {
@@ -77,6 +79,9 @@ private:
     int32_t imageWidth;
     int32_t imageHeight;
 
+    bool _is3D;
+    QVector< QVector<int32_t> > heightModel;
+
     float maxU;
     float maxV;
 };

src/ui/map3D/TextureCache.cc

@@ -52,19 +52,20 @@ TextureCache::TextureCache(uint32_t _cacheSize)
 }
 
 TexturePtr
-TextureCache::get(const QString& tileURL)
+TextureCache::get(const QString& tileURL, bool useHeightModel)
 {
-    QPair<TexturePtr, int32_t> p1 = lookup(tileURL);
+    QPair<TexturePtr, int32_t> p1 = lookup(tileURL, useHeightModel);
     if (!p1.first.isNull())
     {
         return p1.first;
     }
 
-    QPair<WebImagePtr, int32_t> p2 = imageCache->lookup(tileURL);
+    QPair<WebImagePtr, int32_t> p2 =
+            imageCache->lookup(tileURL, useHeightModel);
     if (!p2.first.isNull())
     {
         textures[p2.second]->sync(p2.first);
-        p1 = lookup(tileURL);
+        p1 = lookup(tileURL, useHeightModel);
 
         return p1.first;
     }
@@ -85,11 +86,12 @@ TextureCache::sync(void)
 }
 
 QPair<TexturePtr, int32_t>
-TextureCache::lookup(const QString& tileURL)
+TextureCache::lookup(const QString& tileURL, bool useHeightModel)
 {
     for (int32_t i = 0; i < textures.size(); ++i)
     {
-        if (textures[i]->getSourceURL() == tileURL)
+        if (textures[i]->getSourceURL() == tileURL &&
+            textures[i]->is3D() == useHeightModel)
         {
             return qMakePair(textures[i], i);
         }

src/ui/map3D/TextureCache.h

@@ -42,12 +42,13 @@ class TextureCache
 public:
     explicit TextureCache(uint32_t cacheSize);
 
-    TexturePtr get(const QString& tileURL);
+    TexturePtr get(const QString& tileURL, bool useHeightModel = false);
 
     void sync(void);
 
 private:
-    QPair<TexturePtr, int32_t> lookup(const QString& tileURL);
+    QPair<TexturePtr, int32_t> lookup(const QString& tileURL,
+                                      bool useHeightModel);
 
     bool requireSync(void) const;
 

src/ui/map3D/WebImage.cc

@@ -31,7 +31,7 @@ This file is part of the QGROUNDCONTROL project
 
 #include "WebImage.h"
 
-#include <QDebug>
+#include <QFile>
 #include <QGLWidget>
 
 WebImage::WebImage()
@@ -39,6 +39,7 @@ WebImage::WebImage()
     , sourceURL("")
     , image(0)
     , lastReference(0)
+    , _is3D(false)
     , syncFlag(false)
 {
 
@@ -51,6 +52,7 @@ WebImage::clear(void)
     sourceURL.clear();
     state = WebImage::UNINITIALIZED;
     lastReference = 0;
+    heightModel.clear();
 }
 
 WebImage::State
@@ -78,12 +80,18 @@ WebImage::setSourceURL(const QString& url)
 }
 
 const uint8_t*
-WebImage::getData(void) const
+WebImage::getImageData(void) const
 {
     return image->scanLine(0);
 }
 
-void
+const QVector< QVector<int32_t> >&
+WebImage::getHeightModel(void) const
+{
+    return heightModel;
+}
+
+bool
 WebImage::setData(const QByteArray& data)
 {
     QImage tempImage;
@@ -94,10 +102,84 @@ WebImage::setData(const QByteArray& data)
             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
     {
-        qDebug() << "# WARNING: cannot load image data for" << sourceURL;
+        return false;
     }
 }
 
@@ -119,6 +201,12 @@ WebImage::getByteCount(void) const
     return image->byteCount();
 }
 
+bool
+WebImage::is3D(void) const
+{
+    return _is3D;
+}
+
 uint64_t
 WebImage::getLastReference(void) const
 {

src/ui/map3D/WebImage.h

@@ -57,13 +57,18 @@ public:
     const QString& getSourceURL(void) const;
     void setSourceURL(const QString& url);
 
-    const uint8_t* getData(void) const;
-    void setData(const QByteArray& data);
+    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);
 
@@ -74,7 +79,9 @@ private:
     State state;
     QString sourceURL;
     QScopedPointer<QImage> image;
+    QVector< QVector<int32_t> > heightModel;
     uint64_t lastReference;
+    bool _is3D;
     bool syncFlag;
 };
 

src/ui/map3D/WebImageCache.cc

@@ -52,14 +52,15 @@ WebImageCache::WebImageCache(QObject* parent, uint32_t _cacheSize)
 }
 
 QPair<WebImagePtr, int32_t>
-WebImageCache::lookup(const QString& url)
+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]->getSourceURL() == url &&
+            webImages[i]->is3D() == useHeightModel)
         {
             cacheEntry.first = webImages[i];
             cacheEntry.second = i;
@@ -104,7 +105,37 @@ WebImageCache::lookup(const QString& url)
             ++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;
         }

src/ui/map3D/WebImageCache.h

@@ -45,7 +45,8 @@ class WebImageCache : public QObject
 public:
     WebImageCache(QObject* parent, uint32_t cacheSize);
 
-    QPair<WebImagePtr, int32_t> lookup(const QString& url);
+    QPair<WebImagePtr, int32_t> lookup(const QString& url,
+                                       bool useHeightModel);
 
     WebImagePtr at(int32_t index) const;