diff --git a/map.earth b/map.earth new file mode 100644 index 0000000000000000000000000000000000000000..4ee5ea3dee0c98caeec63a8dc6f5d61cecd72ee7 --- /dev/null +++ b/map.earth @@ -0,0 +1,7 @@ + + + + ../map/zurich/tms.xml + + + diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 56d85d197c30fda5ca53b3ad2218be405c1adfea..f049ba072df815a9d1ceabadc5d99c275f83bea7 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -220,14 +220,10 @@ HEADERS += src/MG.h \ src/ui/RadioCalibration/AbstractCalibrator.h \ src/ui/map3D/Q3DWidget.h \ src/ui/map3D/PixhawkCheetahGeode.h \ - src/ui/map3D/Texture.h \ - src/ui/map3D/TextureCache.h \ - src/ui/map3D/WebImage.h \ - src/ui/map3D/WebImageCache.h \ - src/ui/map3D/Imagery.h \ src/comm/QGCMAVLink.h \ src/ui/map3D/Pixhawk3DWidget.h \ - src/ui/map3D/Q3DWidgetFactory.h + src/ui/map3D/Q3DWidgetFactory.h \ + src/ui/map3D/GCManipulator.h SOURCES += src/main.cc \ src/Core.cc \ src/uas/UASManager.cc \ @@ -300,13 +296,9 @@ SOURCES += src/main.cc \ src/ui/WaypointGlobalView.cc \ src/ui/map3D/Q3DWidget.cc \ src/ui/map3D/PixhawkCheetahGeode.cc \ - src/ui/map3D/Texture.cc \ - src/ui/map3D/TextureCache.cc \ - src/ui/map3D/WebImageCache.cc \ - src/ui/map3D/WebImage.cc \ - src/ui/map3D/Imagery.cc \ src/ui/map3D/Pixhawk3DWidget.cc \ - src/ui/map3D/Q3DWidgetFactory.cc + src/ui/map3D/Q3DWidgetFactory.cc \ + src/ui/map3D/GCManipulator.cc RESOURCES = mavground.qrc # Include RT-LAB Library diff --git a/src/ui/map3D/GCManipulator.cc b/src/ui/map3D/GCManipulator.cc new file mode 100644 index 0000000000000000000000000000000000000000..cdabc966533ea9f53448713144bcbd9b584f5e6b --- /dev/null +++ b/src/ui/map3D/GCManipulator.cc @@ -0,0 +1,307 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +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 . + +======================================================================*/ + +/** + * @file + * @brief Definition of the class GCManipulator. + * + * @author Lionel Heng + * + */ + +#include "GCManipulator.h" + +GCManipulator::GCManipulator() +{ + _moveSensitivity = 0.05f; + _zoomSensitivity = 1.0f; + _minZoomRange = 2.0f; +} + +void +GCManipulator::setMinZoomRange(float minZoomRange) +{ + _minZoomRange = minZoomRange; +} + +void +GCManipulator::move(float dx, float dy, float dz) +{ + _center += osg::Vec3(dx, dy, dz); +} + +bool +GCManipulator::handle(const osgGA::GUIEventAdapter& ea, + osgGA::GUIActionAdapter& us) +{ + using namespace osgGA; + + switch (ea.getEventType()) + { + case GUIEventAdapter::PUSH: + { + flushMouseEventStack(); + addMouseEvent(ea); + if (calcMovement()) + { + us.requestRedraw(); + } + us.requestContinuousUpdate(false); + _thrown = false; + return true; + } + + case GUIEventAdapter::RELEASE: + { + if (ea.getButtonMask() == 0) + { + if (isMouseMoving()) + { + if (calcMovement()) + { + us.requestRedraw(); + us.requestContinuousUpdate(true); + _thrown = true; + } + } + else + { + flushMouseEventStack(); + addMouseEvent(ea); + if (calcMovement()) + { + us.requestRedraw(); + } + us.requestContinuousUpdate(false); + _thrown = false; + } + + } + else + { + flushMouseEventStack(); + addMouseEvent(ea); + if (calcMovement()) + { + us.requestRedraw(); + } + us.requestContinuousUpdate(false); + _thrown = false; + } + return true; + } + + case GUIEventAdapter::DRAG: + { + addMouseEvent(ea); + if (calcMovement()) + { + us.requestRedraw(); + } + us.requestContinuousUpdate(false); + _thrown = false; + return true; + } + + case GUIEventAdapter::SCROLL: + { + // zoom model + float scale = 1.0f; + + if (ea.getScrollingMotion() == GUIEventAdapter::SCROLL_UP) + { + scale -= _zoomSensitivity * 0.1f; + } + else + { + scale += _zoomSensitivity * 0.1f; + } + if (_distance * scale > _minZoomRange) + { + + _distance *= scale; + + } + + return true; + } + + case GUIEventAdapter::KEYDOWN: + // pan model + switch (ea.getKey()) + { + case GUIEventAdapter::KEY_Space: + { + flushMouseEventStack(); + _thrown = false; + home(ea,us); + us.requestRedraw(); + us.requestContinuousUpdate(false); + return true; + } + case GUIEventAdapter::KEY_Left: + { + float scale = -_moveSensitivity * _distance; + + osg::Matrix rotation_matrix; + rotation_matrix.makeRotate(_rotation); + + osg::Vec3 dv(scale, 0.0f, 0.0f); + + _center += dv * rotation_matrix; + + return true; + } + case GUIEventAdapter::KEY_Right: + { + float scale = _moveSensitivity * _distance; + + osg::Matrix rotation_matrix; + rotation_matrix.makeRotate(_rotation); + + osg::Vec3 dv(scale, 0.0f, 0.0f); + + _center += dv * rotation_matrix; + + return true; + } + case GUIEventAdapter::KEY_Up: + { + float scale = _moveSensitivity * _distance; + + osg::Matrix rotation_matrix; + rotation_matrix.makeRotate(_rotation); + + osg::Vec3 dv(0.0f, scale, 0.0f); + + _center += dv * rotation_matrix; + + return true; + } + case GUIEventAdapter::KEY_Down: + { + float scale = -_moveSensitivity * _distance; + + osg::Matrix rotation_matrix; + rotation_matrix.makeRotate(_rotation); + + osg::Vec3 dv(0.0f, scale, 0.0f); + + _center += dv * rotation_matrix; + + return true; + } + return false; + } + + case GUIEventAdapter::FRAME: + if (_thrown) + { + if (calcMovement()) + { + us.requestRedraw(); + } + } + return false; + + default: + return false; + } +} + + +bool +GCManipulator::calcMovement() +{ + using namespace osgGA; + + // return if less then two events have been added. + if (_ga_t0.get() == NULL || _ga_t1.get() == NULL) + { + return false; + } + + float dx = _ga_t0->getXnormalized() - _ga_t1->getXnormalized(); + float dy = _ga_t0->getYnormalized() - _ga_t1->getYnormalized(); + + // return if there is no movement. + if (dx == 0.0f && dy == 0.0f) + { + return false; + } + + unsigned int buttonMask = _ga_t1->getButtonMask(); + if (buttonMask == GUIEventAdapter::LEFT_MOUSE_BUTTON) + { + // rotate camera + osg::Vec3 axis; + float angle; + + float px0 = _ga_t0->getXnormalized(); + float py0 = _ga_t0->getYnormalized(); + + float px1 = _ga_t1->getXnormalized(); + float py1 = _ga_t1->getYnormalized(); + + trackball(axis, angle, px1, py1, px0, py0); + + osg::Quat new_rotate; + new_rotate.makeRotate(angle, axis); + + _rotation = _rotation * new_rotate; + + return true; + + } + else if (buttonMask == GUIEventAdapter::MIDDLE_MOUSE_BUTTON || + buttonMask == (GUIEventAdapter::LEFT_MOUSE_BUTTON | + GUIEventAdapter::RIGHT_MOUSE_BUTTON)) + { + // pan model + float scale = -_moveSensitivity * _distance; + + osg::Matrix rotation_matrix; + rotation_matrix.makeRotate(_rotation); + + osg::Vec3 dv(dx * scale, dy * scale, 0.0f); + + _center += dv * rotation_matrix; + + return true; + + } + else if (buttonMask == GUIEventAdapter::RIGHT_MOUSE_BUTTON) + { + // zoom model + float scale = 1.0f + dy * _zoomSensitivity; + if (_distance * scale > _minZoomRange) + { + + _distance *= scale; + + } + return true; + } + + return false; +} diff --git a/src/ui/map3D/TextureCache.h b/src/ui/map3D/GCManipulator.h similarity index 61% rename from src/ui/map3D/TextureCache.h rename to src/ui/map3D/GCManipulator.h index ef469a124ea11f352e60d43cb6044dd0cb9f3836..87477813a8ecdbf301f9f8fe3fa75fcc8ec38056 100644 --- a/src/ui/map3D/TextureCache.h +++ b/src/ui/map3D/GCManipulator.h @@ -23,39 +23,39 @@ This file is part of the QGROUNDCONTROL project /** * @file - * @brief Definition of the class TextureCache. + * @brief Definition of the class GCManipulator. * * @author Lionel Heng * */ -#ifndef TEXTURECACHE_H -#define TEXTURECACHE_H +#ifndef GCMANIPULATOR_H +#define GCMANIPULATOR_H -#include +#include -#include "Texture.h" -#include "WebImageCache.h" - -class TextureCache +class GCManipulator : public osgGA::TrackballManipulator { public: - explicit TextureCache(uint32_t cacheSize); - - TexturePtr get(const QString& tileURL, bool useHeightModel = false); + GCManipulator(); - void sync(void); + void setMinZoomRange(float minZoomRange); -private: - QPair lookup(const QString& tileURL, - bool useHeightModel); + virtual void move(float dx, float dy, float dz); - bool requireSync(void) const; + /** + * @brief Handle events. + * @return True if event is handled; false otherwise. + */ + virtual bool handle(const osgGA::GUIEventAdapter& ea, + osgGA::GUIActionAdapter& us); - uint32_t cacheSize; - QVector textures; +protected: + bool calcMovement(); - QScopedPointer imageCache; + float _moveSensitivity; + float _zoomSensitivity; + float _minZoomRange; }; -#endif // TEXTURECACHE_H +#endif // GCMANIPULATOR_H diff --git a/src/ui/map3D/Imagery.cc b/src/ui/map3D/Imagery.cc deleted file mode 100644 index 1b7bacbe7a4bd59d30ca65c7f5ab2f0ad47a21ec..0000000000000000000000000000000000000000 --- a/src/ui/map3D/Imagery.cc +++ /dev/null @@ -1,594 +0,0 @@ -/*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 QGROUNDCONTROL PROJECT - -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 . - -======================================================================*/ - -/** - * @file - * @brief Definition of the class Imagery. - * - * @author Lionel Heng - * - */ - -#include "Imagery.h" - -#include -#include -#include - -const double WGS84_A = 6378137.0; -const double WGS84_ECCSQ = 0.00669437999013; - -const int32_t 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; - } - - int32_t minTileX, minTileY, maxTileX, maxTileY; - int32_t 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 (int32_t r = minTileY; r <= maxTileY; ++r) - { - for (int32_t 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; - } - - int32_t minTileX, minTileY, maxTileX, maxTileY; - int32_t 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 (int32_t r = minTileY; r <= maxTileY; ++r) - { - for (int32_t 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) -{ - int32_t minTileX, minTileY, maxTileX, maxTileY; - int32_t zoomLevel; - - tileBounds(tileResolution, - xOrigin + viewXOffset - radius, - yOrigin + viewYOffset - radius, - xOrigin + viewXOffset + radius, - yOrigin + viewYOffset + radius, utmZone, - minTileX, minTileY, maxTileX, maxTileY, zoomLevel); - - for (int32_t r = minTileY; r <= maxTileY; ++r) - { - for (int32_t 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) -{ - int32_t minTileX, minTileY, maxTileX, maxTileY; - int32_t zoomLevel; - - tileBounds(tileResolution, - xOrigin + viewXOffset - radius, - yOrigin + viewYOffset - radius, - xOrigin + viewXOffset + radius, - yOrigin + viewYOffset + radius, utmZone, - minTileX, minTileY, maxTileX, maxTileY, zoomLevel); - - for (int32_t r = minTileY; r <= maxTileY; ++r) - { - for (int32_t 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(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(rint(log2(tileResolution))); - int32_t numTiles = static_cast(exp2(static_cast(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, - int32_t& minTileX, int32_t& minTileY, - int32_t& maxTileX, int32_t& maxTileY, - int32_t& zoomLevel) const -{ - 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(rint(minUtmX / utmMultiplier)); - minTileY = static_cast(rint(minUtmY / utmMultiplier)); - centerTileX = static_cast(rint(centerUtmX / utmMultiplier)); - centerTileY = static_cast(rint(centerUtmY / utmMultiplier)); - maxTileX = static_cast(rint(maxUtmX / utmMultiplier)); - maxTileY = static_cast(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(int32_t tileX, int32_t numTiles) const -{ - return 360.0 * (static_cast(tileX) - / static_cast(numTiles)) - 180.0; -} - -double -Imagery::tileYToLatitude(int32_t tileY, int32_t numTiles) const -{ - double unnormalizedRad = - (static_cast(tileY) / static_cast(numTiles)) - * 2.0 * M_PI - M_PI; - double rad = 2.0 * atan(exp(unnormalizedRad)) - M_PI / 2.0; - return -rad * 180.0 / M_PI; -} - -int32_t -Imagery::longitudeToTileX(double longitude, int32_t numTiles) const -{ - return static_cast((longitude / 180.0 + 1.0) / 2.0 * numTiles); -} - -int32_t -Imagery::latitudeToTileY(double latitude, int32_t numTiles) const -{ - double rad = latitude * M_PI / 180.0; - double normalizedRad = -log(tan(rad) + 1.0 / cos(rad)); - return static_cast((normalizedRad + M_PI) - / (2.0 * M_PI) * numTiles); -} - -void -Imagery::UTMtoTile(double northing, double easting, const QString& utmZone, - double tileResolution, int32_t& tileX, int32_t& tileY, - int32_t& zoomLevel) const -{ - double latitude, longitude; - - UTMtoLL(northing, easting, utmZone, latitude, longitude); - - zoomLevel = MAX_ZOOM_LEVEL - static_cast(rint(log2(tileResolution))); - int32_t numTiles = static_cast(exp2(static_cast(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; - - int32_t ZoneNumber = static_cast((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((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; - int32_t 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(int32_t tileX, int32_t tileY, int32_t 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(rint(tileResolution)); - } - oss << "-" << tileY << "-" << tileX << ".jpg"; - default: - {}; - } - - QString url(oss.str().c_str()); - - return url; -} diff --git a/src/ui/map3D/Imagery.h b/src/ui/map3D/Imagery.h deleted file mode 100644 index e0d8d3b3e462a39313c476e45d49330f592eeb4b..0000000000000000000000000000000000000000 --- a/src/ui/map3D/Imagery.h +++ /dev/null @@ -1,115 +0,0 @@ -/*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 QGROUNDCONTROL PROJECT - -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 . - -======================================================================*/ - -/** - * @file - * @brief Definition of the class Imagery. - * - * @author Lionel Heng - * - */ - -#ifndef IMAGERY_H -#define IMAGERY_H - -#include - -#include "TextureCache.h" - -class Imagery -{ -public: - Imagery(); - - enum ImageryType - { - GOOGLE_MAP = 0, - GOOGLE_SATELLITE = 1, - SWISSTOPO_SATELLITE = 2, - SWISSTOPO_SATELLITE_3D = 3 - }; - - 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(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; - - void tileBounds(double tileResolution, - double minUtmX, double minUtmY, - double maxUtmX, double maxUtmY, const QString& utmZone, - int32_t& minTileX, int32_t& minTileY, - int32_t& maxTileX, int32_t& maxTileY, - int32_t& zoomLevel) const; - - double tileXToLongitude(int32_t tileX, int32_t numTiles) const; - double tileYToLatitude(int32_t tileY, int32_t numTiles) const; - int32_t longitudeToTileX(double longitude, int32_t numTiles) const; - int32_t latitudeToTileY(double latitude, int32_t numTiles) const; - - void UTMtoTile(double northing, double easting, const QString& utmZone, - double tileResolution, int32_t& tileX, int32_t& tileY, - int32_t& 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(int32_t tileX, int32_t tileY, int32_t zoomLevel, - double tileResolution) const; - - ImageryType currentImageryType; - - QScopedPointer textureCache; - - double xOffset, yOffset; -}; - -#endif // IMAGERY_H diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index 450b4944dadc8a108fb4df8ad4ba8eb9551de65d..2bbabe355765bcc92775c6b20b7b27af1bf44920 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/src/ui/map3D/Pixhawk3DWidget.cc @@ -31,7 +31,6 @@ #include "Pixhawk3DWidget.h" -#include #include #include @@ -46,15 +45,17 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent) : Q3DWidget(parent) , uas(NULL) - , lastRedrawTime(0.0) , displayGrid(true) , displayTrail(false) , displayTarget(false) , displayWaypoints(true) - , lockCamera(true) + , followCamera(true) + , lastRobotX(0.0f) + , lastRobotY(0.0f) + , lastRobotZ(0.0f) { + setCameraParams(2.0f, 30.0f, 0.01f, 10000.0f); init(15.0f); - setCameraParams(0.5f, 30.0f, 0.01f, 10000.0f); // generate Pixhawk Cheetah model egocentricMap->addChild(PixhawkCheetahGeode::instance()); @@ -72,8 +73,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent) root->addChild(mapNode); // generate target model - targetNode = createTarget(); - rollingMap->addChild(targetNode); + allocentricMap->addChild(createTarget()); // generate waypoint model waypointsNode = createWaypoints(); @@ -81,10 +81,6 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent) setupHUD(); - setDisplayFunc(display, this); - setMouseFunc(mouse, this); - addTimerFunc(100, timer, this); - buildLayout(); connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), @@ -113,14 +109,15 @@ Pixhawk3DWidget::buildLayout(void) 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* lockCameraCheckBox = new QCheckBox(this); - lockCameraCheckBox->setText("Lock Camera"); - lockCameraCheckBox->setChecked(lockCamera); + QCheckBox* followCameraCheckBox = new QCheckBox(this); + followCameraCheckBox->setText("Follow Camera"); + followCameraCheckBox->setChecked(followCamera); QGridLayout* layout = new QGridLayout(this); layout->setMargin(0); @@ -132,7 +129,7 @@ Pixhawk3DWidget::buildLayout(void) layout->addWidget(targetButton, 1, 4); layout->addItem(new QSpacerItem(20, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 1, 5); layout->addWidget(recenterButton, 1, 6); - layout->addWidget(lockCameraCheckBox, 1, 7); + layout->addWidget(followCameraCheckBox, 1, 7); layout->setRowStretch(0, 100); layout->setRowStretch(1, 1); setLayout(layout); @@ -143,97 +140,9 @@ Pixhawk3DWidget::buildLayout(void) this, SLOT(showTrail(int))); connect(waypointsCheckBox, SIGNAL(stateChanged(int)), this, SLOT(showWaypoints(int))); - connect(recenterButton, SIGNAL(clicked()), this, SLOT(recenterCamera())); - connect(lockCameraCheckBox, SIGNAL(stateChanged(int)), - this, SLOT(toggleLockCamera(int))); -} - -void -Pixhawk3DWidget::display(void* clientData) -{ - Pixhawk3DWidget* map3d = reinterpret_cast(clientData); - map3d->displayHandler(); -} - -void -Pixhawk3DWidget::displayHandler(void) -{ - float robotX = 0.0f, robotY = 0.0f, robotZ = 0.0f; - float robotRoll = 0.0f, robotPitch = 0.0f, robotYaw = 0.0f; - if (uas != NULL) - { - robotX = uas->getLocalX(); - robotY = uas->getLocalY(); - robotZ = uas->getLocalZ(); - robotRoll = uas->getRoll(); - robotPitch = uas->getPitch(); - robotYaw = uas->getYaw(); - } - - robotPosition->setPosition(osg::Vec3(robotY, robotX, -robotZ)); - robotAttitude->setAttitude(osg::Quat(-robotYaw, osg::Vec3f(0.0f, 0.0f, 1.0f), - 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(robotX, robotY, robotZ); - updateWaypoints(); - - // set node visibility - rollingMap->setChildValue(gridNode, displayGrid); - rollingMap->setChildValue(trailNode, displayTrail); - rollingMap->setChildValue(targetNode, displayTarget); - rollingMap->setChildValue(waypointsNode, displayWaypoints); -} - -void -Pixhawk3DWidget::mouse(Qt::MouseButton button, MouseState state, - int32_t x, int32_t y, void* clientData) -{ - Pixhawk3DWidget* map3d = reinterpret_cast(clientData); - map3d->mouseHandler(button, state, x, y); -} - -void -Pixhawk3DWidget::mouseHandler(Qt::MouseButton button, MouseState state, - int32_t x, int32_t y) -{ - if (button == Qt::LeftButton && state == MOUSE_STATE_DOWN && - targetButton->isChecked()) - { - markTarget(); - } -} - -void -Pixhawk3DWidget::timer(void* clientData) -{ - Pixhawk3DWidget* map3d = reinterpret_cast(clientData); - map3d->timerHandler(); -} - -void -Pixhawk3DWidget::timerHandler(void) -{ - double timeLapsed = getTime() - lastRedrawTime; - if (timeLapsed > 0.1) - { - forceRedraw(); - lastRedrawTime = getTime(); - } - addTimerFunc(100, timer, this); -} - -double -Pixhawk3DWidget::getTime(void) const -{ - struct timeval tv; - - gettimeofday(&tv, NULL); - - return static_cast(tv.tv_sec) + - static_cast(tv.tv_usec) / 1000000.0; + connect(recenterButton, SIGNAL(clicked()), this, SLOT(recenter())); + connect(followCameraCheckBox, SIGNAL(stateChanged(int)), + this, SLOT(toggleFollowCamera(int))); } /** @@ -271,7 +180,7 @@ Pixhawk3DWidget::showTrail(int32_t state) { if (!displayTrail) { - trailVertices->clear(); + trail.clear(); } displayTrail = true; @@ -296,60 +205,161 @@ Pixhawk3DWidget::showWaypoints(int state) } void -Pixhawk3DWidget::recenterCamera(void) +Pixhawk3DWidget::recenter(void) { - recenter(); + float robotX = 0.0f, robotY = 0.0f, robotZ = 0.0f; + if (uas != NULL) + { + robotX = uas->getLocalX(); + robotY = uas->getLocalY(); + robotZ = uas->getLocalZ(); + } + + recenterCamera(robotY, robotX, -robotZ); } void -Pixhawk3DWidget::toggleLockCamera(int32_t state) +Pixhawk3DWidget::toggleFollowCamera(int32_t state) { if (state == Qt::Checked) { - lockCamera = true; + followCamera = true; } else { - lockCamera = false; + followCamera = false; + } +} + +void +Pixhawk3DWidget::display(void) +{ + if (uas == NULL) + { + return; + } + + float robotX = uas->getLocalX(); + float robotY = uas->getLocalY(); + float robotZ = uas->getLocalZ(); + float robotRoll = uas->getRoll(); + float robotPitch = uas->getPitch(); + float robotYaw = uas->getYaw(); + + if (lastRobotX == 0.0f && lastRobotY == 0.0f && lastRobotZ == 0.0f) + { + lastRobotX = robotX; + lastRobotY = robotY; + lastRobotZ = robotZ; + + recenterCamera(robotY, robotX, -robotZ); + + return; + } + + if (followCamera) + { + float dx = robotY - lastRobotY; + float dy = robotX - lastRobotX; + float dz = lastRobotZ - robotZ; + + moveCamera(dx, dy, dz); + } + + robotPosition->setPosition(osg::Vec3(robotY, robotX, -robotZ)); + robotAttitude->setAttitude(osg::Quat(-robotYaw, osg::Vec3f(0.0f, 0.0f, 1.0f), + 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(); + + // set node visibility + rollingMap->setChildValue(gridNode, displayGrid); + rollingMap->setChildValue(trailNode, displayTrail); + rollingMap->setChildValue(targetNode, displayTarget); + rollingMap->setChildValue(waypointsNode, displayWaypoints); + + lastRobotX = robotX; + lastRobotY = robotY; + lastRobotZ = robotZ; +} + +void +Pixhawk3DWidget::mousePressEvent(QMouseEvent* event) +{ + if (event->button() == Qt::LeftButton && targetButton->isChecked()) + { + markTarget(); } + + Q3DWidget::mousePressEvent(event); } osg::ref_ptr Pixhawk3DWidget::createGrid(void) { osg::ref_ptr geode(new osg::Geode()); - osg::ref_ptr geometry(new osg::Geometry()); - geode->addDrawable(geometry.get()); + osg::ref_ptr fineGeometry(new osg::Geometry()); + osg::ref_ptr coarseGeometry(new osg::Geometry()); + geode->addDrawable(fineGeometry); + geode->addDrawable(coarseGeometry); float radius = 10.0f; float resolution = 0.25f; - osg::ref_ptr coords(new osg::Vec3Array); + osg::ref_ptr fineCoords(new osg::Vec3Array); + osg::ref_ptr coarseCoords(new osg::Vec3Array); // draw a 20m x 20m grid with 0.25m resolution for (float i = -radius; i <= radius; i += resolution) { - coords->push_back(osg::Vec3(i, -radius, 0.0f)); - coords->push_back(osg::Vec3(i, radius, 0.0f)); - coords->push_back(osg::Vec3(-radius, i, 0.0f)); - coords->push_back(osg::Vec3(radius, i, 0.0f)); + if (fabsf(i - roundf(i)) < 0.01f) + { + coarseCoords->push_back(osg::Vec3(i, -radius, 0.0f)); + coarseCoords->push_back(osg::Vec3(i, radius, 0.0f)); + coarseCoords->push_back(osg::Vec3(-radius, i, 0.0f)); + coarseCoords->push_back(osg::Vec3(radius, i, 0.0f)); + } + else + { + fineCoords->push_back(osg::Vec3(i, -radius, 0.0f)); + fineCoords->push_back(osg::Vec3(i, radius, 0.0f)); + fineCoords->push_back(osg::Vec3(-radius, i, 0.0f)); + fineCoords->push_back(osg::Vec3(radius, i, 0.0f)); + } } - geometry->setVertexArray(coords); + fineGeometry->setVertexArray(fineCoords); + coarseGeometry->setVertexArray(coarseCoords); osg::ref_ptr color(new osg::Vec4Array); color->push_back(osg::Vec4(0.5f, 0.5f, 0.5f, 1.0f)); - geometry->setColorArray(color); - geometry->setColorBinding(osg::Geometry::BIND_OVERALL); - - geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, coords->size())); - - osg::ref_ptr stateset(new osg::StateSet); - osg::ref_ptr linewidth(new osg::LineWidth()); - linewidth->setWidth(0.25f); - stateset->setAttributeAndModes(linewidth, osg::StateAttribute::ON); - stateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF); - geometry->setStateSet(stateset); + fineGeometry->setColorArray(color); + coarseGeometry->setColorArray(color); + fineGeometry->setColorBinding(osg::Geometry::BIND_OVERALL); + coarseGeometry->setColorBinding(osg::Geometry::BIND_OVERALL); + + fineGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, + 0, fineCoords->size())); + coarseGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, + coarseCoords->size())); + + osg::ref_ptr fineStateset(new osg::StateSet); + osg::ref_ptr fineLinewidth(new osg::LineWidth()); + fineLinewidth->setWidth(0.25f); + fineStateset->setAttributeAndModes(fineLinewidth, osg::StateAttribute::ON); + fineStateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF); + fineGeometry->setStateSet(fineStateset); + + osg::ref_ptr coarseStateset(new osg::StateSet); + osg::ref_ptr coarseLinewidth(new osg::LineWidth()); + coarseLinewidth->setWidth(2.0f); + coarseStateset->setAttributeAndModes(coarseLinewidth, osg::StateAttribute::ON); + coarseStateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF); + coarseGeometry->setStateSet(coarseStateset); return geode; } @@ -392,12 +402,15 @@ Pixhawk3DWidget::createMap(void) return node; } -osg::ref_ptr +osg::ref_ptr Pixhawk3DWidget::createTarget(void) { - osg::ref_ptr group(new osg::Group()); + targetPosition = new osg::PositionAttitudeTransform; - return group; + targetNode = new osg::Geode; + targetPosition->addChild(targetNode); + + return targetPosition; } osg::ref_ptr @@ -543,7 +556,7 @@ Pixhawk3DWidget::updateTrail(float robotX, float robotY, float robotZ) } void -Pixhawk3DWidget::updateTarget(float robotX, float robotY, float robotZ) +Pixhawk3DWidget::updateTarget(void) { static double radius = 0.2; static bool expand = true; @@ -557,9 +570,9 @@ Pixhawk3DWidget::updateTarget(float robotX, float robotY, float robotZ) expand = false; } - if (targetNode->getNumChildren() > 0) + if (targetNode->getNumDrawables() > 0) { - targetNode->removeChild(0, targetNode->getNumChildren()); + targetNode->removeDrawables(0, targetNode->getNumDrawables()); } osg::ref_ptr sd = new osg::ShapeDrawable; @@ -568,18 +581,7 @@ Pixhawk3DWidget::updateTarget(float robotX, float robotY, float robotZ) sd->setShape(sphere); sd->setColor(osg::Vec4(0.0f, 0.7f, 1.0f, 1.0f)); - osg::ref_ptr geode = new osg::Geode; - geode->addDrawable(sd); - - osg::ref_ptr pat = - new osg::PositionAttitudeTransform; - - pat->setPosition(osg::Vec3d(targetPosition.y() - robotY, - targetPosition.x() - robotX, - 0.0)); - - targetNode->addChild(pat); - pat->addChild(geode); + targetNode->addDrawable(sd); if (expand) { @@ -645,21 +647,20 @@ Pixhawk3DWidget::markTarget(void) robotZ = uas->getLocalZ(); } - std::pair mouseWorldCoords = - getGlobalCursorPosition(getLastMouseX(), getLastMouseY(), -robotZ); + std::pair cursorWorldCoords = + getGlobalCursorPosition(getMouseX(), getMouseY(), -robotZ); + + double targetX = cursorWorldCoords.first; + double targetY = cursorWorldCoords.second; + double targetZ = robotZ; - targetPosition.x() = mouseWorldCoords.first; - targetPosition.y() = mouseWorldCoords.second; - targetPosition.z() = robotZ; + targetPosition->setPosition(osg::Vec3d(targetY, targetX, -targetZ)); displayTarget = true; if (uas) { - uas->setTargetPosition(targetPosition.x(), - targetPosition.y(), - targetPosition.z(), - 0.0f); + uas->setTargetPosition(targetX, targetY, targetZ, 0.0f); } targetButton->setChecked(false); diff --git a/src/ui/map3D/Pixhawk3DWidget.h b/src/ui/map3D/Pixhawk3DWidget.h index f96092c95f9d5aa3a468bd3c216de2b7f966ea20..34e318992910ec50184b450034b642ba469162c5 100644 --- a/src/ui/map3D/Pixhawk3DWidget.h +++ b/src/ui/map3D/Pixhawk3DWidget.h @@ -52,17 +52,6 @@ public: void buildLayout(void); - static void display(void* clientData); - void displayHandler(void); - - static void mouse(Qt::MouseButton button, MouseState state, - int32_t x, int32_t y, void* clientData); - void mouseHandler(Qt::MouseButton button, MouseState state, - int32_t x, int32_t y); - - static void timer(void* clientData); - void timerHandler(void); - double getTime(void) const; public slots: @@ -72,17 +61,20 @@ private slots: void showGrid(int state); void showTrail(int state); void showWaypoints(int state); - void recenterCamera(void); - void toggleLockCamera(int state); + void recenter(void); + void toggleFollowCamera(int state); protected: + virtual void display(void); + virtual void mousePressEvent(QMouseEvent* event); + UASInterface* uas; private: osg::ref_ptr createGrid(void); osg::ref_ptr createTrail(void); osg::ref_ptr createMap(void); - osg::ref_ptr createTarget(void); + osg::ref_ptr createTarget(void); osg::ref_ptr createWaypoints(void); void setupHUD(void); @@ -90,25 +82,21 @@ private: void updateHUD(float robotX, float robotY, float robotZ, float robotRoll, float robotPitch, float robotYaw); void updateTrail(float robotX, float robotY, float robotZ); - void updateTarget(float robotX, float robotY, float robotZ); + void updateTarget(void); void updateWaypoints(void); void markTarget(void); - double lastRedrawTime; - bool displayGrid; bool displayTrail; bool displayTarget; bool displayWaypoints; - bool lockCamera; + bool followCamera; osg::ref_ptr trailVertices; QVarLengthArray trail; - osg::Vec3 targetPosition; - osg::ref_ptr hudBackgroundGeometry; osg::ref_ptr statusText; osg::ref_ptr gridNode; @@ -116,10 +104,13 @@ private: osg::ref_ptr trailGeometry; osg::ref_ptr trailDrawArrays; osg::ref_ptr mapNode; - osg::ref_ptr targetNode; + osg::ref_ptr targetNode; + osg::ref_ptr targetPosition; osg::ref_ptr waypointsNode; QPushButton* targetButton; + + float lastRobotX, lastRobotY, lastRobotZ; }; #endif // PIXHAWK3DWIDGET_H diff --git a/src/ui/map3D/PixhawkCheetahGeode.cc b/src/ui/map3D/PixhawkCheetahGeode.cc index a893fb706b38036db8889a8b41ecebe9493acbe6..9b5251a2f195df2f192474c3776a019b008ea357 100755 --- a/src/ui/map3D/PixhawkCheetahGeode.cc +++ b/src/ui/map3D/PixhawkCheetahGeode.cc @@ -23,7 +23,8 @@ This file is part of the QGROUNDCONTROL project /** * @file - * @brief Generates a geode which renders a Pixhawk Cheetah MAV. + * @brief Generates a OpenSceneGraph geode which renders a + * Pixhawk Cheetah MAV. * * @author Lionel Heng * @@ -48,7 +49,7 @@ static sample_MATERIAL materials [1] = { {{0.117647f,0.117647f,0.117647f}, {0.596078f,0.666667f,0.686275f}, {0.301176f,0.301176f,0.301176f}, {0.0f,0.0f,0.0f}, 1.0f,8.0f,-1} //Material #1 }; -// 38747 Verticies +// 38747 Vertices // 0 Texture Coordinates // 22155 Normals // 77848 Triangles @@ -59372,9 +59373,9 @@ PixhawkCheetahGeode::create(float red, float green, float blue) for (int j = 0; j < 3; ++j) { - int vi = face_indicies[i][j]; - int ni = face_indicies[i][j + 3]; //Normal index - int ti = face_indicies[i][j + 6]; //Texture index + int vi = face_indicies[i][j]; // Vertice index + int ni = face_indicies[i][j + 3]; // Normal index + int ti = face_indicies[i][j + 6]; // Texture index osgVertices->push_back(osg::Vec3(vertices[vi][0], vertices[vi][1], diff --git a/src/ui/map3D/PixhawkCheetahGeode.h b/src/ui/map3D/PixhawkCheetahGeode.h index 933e7009a1c692168fef56f14557b02423603069..d2c7a02b2d0f9043857b629754e1b1f3e741290d 100644 --- a/src/ui/map3D/PixhawkCheetahGeode.h +++ b/src/ui/map3D/PixhawkCheetahGeode.h @@ -23,7 +23,7 @@ This file is part of the QGROUNDCONTROL project /** * @file - * @brief Generates a geode which renders a Pixhawk Cheetah MAV. + * @brief Definition of the class PixhawkCheetahGeode. * * @author Lionel Heng * @@ -35,22 +35,32 @@ This file is part of the QGROUNDCONTROL project #include /** - * @brief Creates a geode which renders a Pixhawk Cheetah MAV. - * @param red Red intensity of the MAV model - * @param green Green intensity of the MAV model - * @param blue Blue intensity of the MAV model - * - * @return The pointer to the geode. + * @brief The PixhawkCheetahGeode class. + * Generates an OpenSceneGraph geode which renders a Pixhawk Cheetah MAV. **/ class PixhawkCheetahGeode { public: + /** + * Constructor. + */ PixhawkCheetahGeode(); + /** + * Get a single instance of the geode. + */ static osg::ref_ptr instance(void); private: + /** + * @brief Creates an OpenSceneGraph geode which renders a Pixhawk Cheetah MAV. + * @param red Red intensity of the MAV model. + * @param green Green intensity of the MAV model. + * @param blue Blue intensity of the MAV model. + * + * @return A smart pointer to the geode. + **/ static osg::ref_ptr create(float red, float green, float blue); static osg::ref_ptr _instance; diff --git a/src/ui/map3D/Q3DWidget.cc b/src/ui/map3D/Q3DWidget.cc index b177f6e3f4179571a12e2419a39484d371e7fd84..6a89afb17cad82c61e399a1cb77be44cb2282ed4 100644 --- a/src/ui/map3D/Q3DWidget.cc +++ b/src/ui/map3D/Q3DWidget.cc @@ -34,7 +34,6 @@ This file is part of the QGROUNDCONTROL project #include #include #include -#include static const float KEY_ROTATE_AMOUNT = 5.0f; static const float KEY_MOVE_AMOUNT = 10.0f; @@ -50,22 +49,9 @@ Q3DWidget::Q3DWidget(QWidget* parent) , robotAttitude(new osg::PositionAttitudeTransform()) , hudGeode(new osg::Geode()) , hudProjectionMatrix(new osg::Projection) - , userDisplayFunc(NULL) - , userKeyboardFunc(NULL) - , userMouseFunc(NULL) - , userMotionFunc(NULL) - , userTimerFunc(NULL) - , userDisplayFuncData(NULL) - , userKeyboardFuncData(NULL) - , userMouseFuncData(NULL) - , userMotionFuncData(NULL) - , userTimerFuncData(NULL) - , lastMouseX(0) - , lastMouseY(0) - , _forceRedraw(false) { // set initial camera parameters - cameraParams.minZoomRange = 0.5f; + cameraParams.minZoomRange = 2.0f; cameraParams.cameraFov = 30.0f; cameraParams.minClipRange = 1.0f; cameraParams.maxClipRange = 10000.0f; @@ -85,11 +71,14 @@ Q3DWidget::~Q3DWidget() void Q3DWidget::init(float fps) { - getCamera()->setGraphicsContext(getGraphicsWindow()); + getCamera()->setGraphicsContext(osgGW); setLightingMode(osg::View::SKY_LIGHT); - // set up coordinate systems + // set up various maps + // allocentric - world map + // rolling - map aligned to the world axes and centered on the robot + // egocentric - vehicle-centric map root->addChild(allocentricMap); allocentricMap->addChild(robotPosition); robotPosition->addChild(rollingMap); @@ -101,12 +90,14 @@ Q3DWidget::init(float fps) // set up HUD root->addChild(createHUD()); + // set up robot egocentricMap->addChild(createRobot()); - osg::ref_ptr nodeTrackerManipulator( - new osgGA::NodeTrackerManipulator); - nodeTrackerManipulator->setTrackNode(rollingMap); - setCameraManipulator(nodeTrackerManipulator); + // set up camera control + cameraManipulator = new GCManipulator; + setCameraManipulator(cameraManipulator); + cameraManipulator->setMinZoomRange(cameraParams.minZoomRange); + cameraManipulator->setDistance(cameraParams.minZoomRange * 2.0); connect(&timer, SIGNAL(timeout()), this, SLOT(redraw())); timer.start(static_cast(floorf(1000.0f / fps))); @@ -187,20 +178,18 @@ Q3DWidget::setCameraParams(float minZoomRange, float cameraFov, cameraParams.cameraFov = cameraFov; cameraParams.minClipRange = minClipRange; cameraParams.maxClipRange = maxClipRange; - - _forceRedraw = true; } void -Q3DWidget::forceRedraw(void) +Q3DWidget::moveCamera(float dx, float dy, float dz) { - _forceRedraw = true; + cameraManipulator->move(dx, dy, dz); } void -Q3DWidget::recenter(void) +Q3DWidget::recenterCamera(float x, float y, float z) { - + cameraManipulator->setCenter(osg::Vec3d(x, y, z)); } void @@ -214,66 +203,20 @@ Q3DWidget::setDisplayMode3D(void) aspect, cameraParams.minClipRange, cameraParams.maxClipRange); - -// getCamera()->setViewMatrixAsLookAt(osg::Vec3d(cameraX + cameraPose.xOffset, -// cameraY + cameraPose.yOffset, -// cameraZ + cameraPose.zOffset), -// osg::Vec3d(cameraPose.xOffset, -// cameraPose.yOffset, -// cameraPose.zOffset), -// osg::Vec3d(0.0, 0.0, 1.0)); -} - -void -Q3DWidget::setDisplayFunc(DisplayFunc func, void* clientData) -{ - userDisplayFunc = func; - userDisplayFuncData = clientData; -} - -void -Q3DWidget::setKeyboardFunc(KeyboardFunc func, void* clientData) -{ - userKeyboardFunc = func; - userKeyboardFuncData = clientData; -} - -void -Q3DWidget::setMouseFunc(MouseFunc func, void* clientData) -{ - userMouseFunc = func; - userMouseFuncData = clientData; -} - -void -Q3DWidget::setMotionFunc(MotionFunc func, void* clientData) -{ - userMotionFunc = func; - userMotionFuncData = clientData; -} - -void -Q3DWidget::addTimerFunc(uint32_t msecs, void(*func)(void *), - void* clientData) -{ - userTimerFunc = func; - userTimerFuncData = clientData; - - QTimer::singleShot(msecs, this, SLOT(userTimer())); } std::pair -Q3DWidget::getGlobalCursorPosition(int32_t mouseX, int32_t mouseY, - double z) +Q3DWidget::getGlobalCursorPosition(int32_t cursorX, int32_t cursorY, double z) { osgUtil::LineSegmentIntersector::Intersections intersections; // normalize cursor position to value between -1 and 1 - double x = -1.0f + static_cast(2 * mouseX) + double x = -1.0f + static_cast(2 * cursorX) / static_cast(width()); - double y = -1.0f + static_cast(2 * (height() - mouseY)) + double y = -1.0f + static_cast(2 * (height() - cursorY)) / static_cast(height()); + // compute matrix which transforms screen coordinates to world coordinates osg::Matrixd m = getCamera()->getViewMatrix() * getCamera()->getProjectionMatrix(); osg::Matrixd invM = osg::Matrixd::inverse(m); @@ -295,20 +238,7 @@ Q3DWidget::getGlobalCursorPosition(int32_t mouseX, int32_t mouseY, void Q3DWidget::redraw(void) { - if (_forceRedraw) - { - updateGL(); - _forceRedraw = false; - } -} - -void -Q3DWidget::userTimer(void) -{ - if (userTimerFunc) - { - userTimerFunc(userTimerFuncData); - } + updateGL(); } int @@ -323,31 +253,8 @@ Q3DWidget::getMouseY(void) return mapFromGlobal(cursor().pos()).y(); } -int -Q3DWidget::getLastMouseX(void) -{ - return lastMouseX; -} - -int -Q3DWidget::getLastMouseY(void) -{ - return lastMouseY; -} - -osgViewer::GraphicsWindow* -Q3DWidget::getGraphicsWindow(void) -{ - return osgGW.get(); -} - -const osgViewer::GraphicsWindow* -Q3DWidget::getGraphicsWindow(void) const -{ - return osgGW.get(); -} - -void Q3DWidget::resizeGL(int width, int height) +void +Q3DWidget::resizeGL(int width, int height) { hudProjectionMatrix->setMatrix(osg::Matrix::ortho2D(0, width, 0, height)); @@ -356,28 +263,34 @@ void Q3DWidget::resizeGL(int width, int height) osgGW->resized(0 , 0, width, height); } -void Q3DWidget::paintGL(void) +void +Q3DWidget::paintGL(void) { setDisplayMode3D(); getCamera()->setClearColor(osg::Vec4f(0.0f, 0.0f, 0.0f, 0.0f)); getCamera()->setClearMask(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - if (userDisplayFunc) - { - userDisplayFunc(userDisplayFuncData); - } + display(); frame(); } -void Q3DWidget::keyPressEvent(QKeyEvent* event) +void +Q3DWidget::display(void) +{ + +} + +void +Q3DWidget::keyPressEvent(QKeyEvent* event) { QWidget::keyPressEvent(event); if (event->isAccepted()) { return; } + if (event->text().isEmpty()) { osgGW->getEventQueue()->keyPress(convertKey(event->key())); @@ -388,24 +301,10 @@ void Q3DWidget::keyPressEvent(QKeyEvent* event) static_cast( *(event->text().toAscii().data()))); } - - if (userKeyboardFunc) - { - if (event->text().isEmpty()) - { - userKeyboardFunc(0, userKeyboardFuncData); - } - else - { - userKeyboardFunc(event->text().at(0).toAscii(), - userKeyboardFuncData); - } - } - - forceRedraw(); } -void Q3DWidget::keyReleaseEvent(QKeyEvent* event) +void +Q3DWidget::keyReleaseEvent(QKeyEvent* event) { QWidget::keyReleaseEvent(event); if (event->isAccepted()) @@ -422,11 +321,10 @@ void Q3DWidget::keyReleaseEvent(QKeyEvent* event) static_cast( *(event->text().toAscii().data()))); } - - forceRedraw(); } -void Q3DWidget::mousePressEvent(QMouseEvent* event) +void +Q3DWidget::mousePressEvent(QMouseEvent* event) { int button = 0; switch (event->button()) @@ -447,17 +345,10 @@ void Q3DWidget::mousePressEvent(QMouseEvent* event) {} } osgGW->getEventQueue()->mouseButtonPress(event->x(), event->y(), button); - - if (userMouseFunc) - { - userMouseFunc(event->button(), MOUSE_STATE_DOWN, event->x(), event->y(), - userMouseFuncData); - } - - forceRedraw(); } -void Q3DWidget::mouseReleaseEvent(QMouseEvent* event) +void +Q3DWidget::mouseReleaseEvent(QMouseEvent* event) { int button = 0; switch (event->button()) @@ -478,35 +369,20 @@ void Q3DWidget::mouseReleaseEvent(QMouseEvent* event) {} } osgGW->getEventQueue()->mouseButtonRelease(event->x(), event->y(), button); - - if (userMouseFunc) - { - userMouseFunc(event->button(), MOUSE_STATE_UP, event->x(), event->y(), - userMouseFuncData); - } - - forceRedraw(); } -void Q3DWidget::mouseMoveEvent(QMouseEvent* event) +void +Q3DWidget::mouseMoveEvent(QMouseEvent* event) { osgGW->getEventQueue()->mouseMotion(event->x(), event->y()); - - if (userMotionFunc) - { - userMotionFunc(event->x(), event->y(), userMotionFuncData); - } - - forceRedraw(); } -void Q3DWidget::wheelEvent(QWheelEvent* event) +void +Q3DWidget::wheelEvent(QWheelEvent* event) { osgGW->getEventQueue()->mouseScroll((event->delta() > 0) ? osgGA::GUIEventAdapter::SCROLL_UP : osgGA::GUIEventAdapter::SCROLL_DOWN); - - forceRedraw(); } float diff --git a/src/ui/map3D/Q3DWidget.h b/src/ui/map3D/Q3DWidget.h index 61ff9128a8bc9c542d82d8ed4ed42f7c5c5d12c7..c1786aa4550b8d2913633f3b0cfd840a9de6c5ce 100644 --- a/src/ui/map3D/Q3DWidget.h +++ b/src/ui/map3D/Q3DWidget.h @@ -36,104 +36,226 @@ This file is part of the QGROUNDCONTROL project #include #include +#include #include -enum MouseState -{ - MOUSE_STATE_UP = 0, - MOUSE_STATE_DOWN = 1 -}; - -typedef void (*DisplayFunc)(void *); -typedef void (*KeyboardFunc)(char, void *); -typedef void (*MouseFunc)(Qt::MouseButton, MouseState, int, int, void *); -typedef void (*MotionFunc)(int, int, void *); +#include "GCManipulator.h" +/** + * @brief Definition of the class Q3DWidget. + * The Q3DWidget widget uses the OpenSceneGraph framework to render + * user-defined objects in 3D. The world coordinate system in OSG is defined: + * - x-axis points to the east + * - y-axis points to the north + * - z-axis points upwards + */ class Q3DWidget : public QGLWidget, public osgViewer::Viewer { Q_OBJECT public: + /** + * Constructor. + * @param parent Parent widget. + */ Q3DWidget(QWidget* parent = 0); + + /** + * Destructor. + */ virtual ~Q3DWidget(); + /** + * @brief Initializes the widget. + * @param fps Frames per second. + */ void init(float fps); - void setCameraParams(float minZoomRange, float cameraFov, - float minClipRange, - float maxClipRange); - void forceRedraw(void); - void recenter(void); + /** + * @brief Sets the camera parameters. + * @param minZoomRange Minimum distance from the viewer to the camera. + * @param cameraFov Camera field of view. + * @param minClipRange Distance from the viewer to the near clipping range. + * @param maxClipRange Distance from the viewer to the far clipping range. + */ + void setCameraParams(float minZoomRange, float cameraFov, + float minClipRange, float maxClipRange); + + /** + * @brief Moves the camera by [dx,dy,dz]. + * @param dx Translation along the x-axis in meters. + * @param dy Translation along the y-axis in meters. + * @param dz Translation along the z-axis in meters. + */ + void moveCamera(float dx, float dy, float dz); + + /** + * @brief Recenters the camera at (x,y,z). + */ + void recenterCamera(float x, float y, float z); + + /** + * @brief Sets up 3D display mode. + */ void setDisplayMode3D(void); - void setDisplayFunc(DisplayFunc func, void* clientData); - void setKeyboardFunc(KeyboardFunc func, void* clientData); - void setMouseFunc(MouseFunc func, void* clientData); - void setMotionFunc(MotionFunc func, void* clientData); - void addTimerFunc(uint msecs, void(*func)(void *), - void* clientData); - - std::pair getGlobalCursorPosition(int32_t mouseX, - int32_t mouseY, + /** + * @brief Gets the world 3D coordinates of the cursor. + * The function projects the 2D cursor position to a line in world space + * and returns the intersection of that line and the horizontal plane + * which contains the point (0, 0, z); + * @param cursorX x-coordinate of the cursor. + * @param cursorY y-coordinate of the cursor. + * @param z z-coordinate of the point in the plane. + * @return A pair of values containing the world 3D cursor coordinates. + */ + std::pair getGlobalCursorPosition(int32_t cursorX, + int32_t cursorY, double z); protected slots: + /** + * @brief Updates the widget. + */ void redraw(void); - void userTimer(void); protected: + /** + * @brief Get base robot geode. + * @return Smart pointer to the geode. + */ osg::ref_ptr createRobot(void); + + /** + * @brief Get base HUD geode. + * @return Smart pointer to the geode. + */ osg::ref_ptr createHUD(void); + /** + * @brief Get screen x-coordinate of mouse cursor. + * @return screen x-coordinate of mouse cursor. + */ int getMouseX(void); - int getMouseY(void); - int getLastMouseX(void); - int getLastMouseY(void); - osgViewer::GraphicsWindow* getGraphicsWindow(void); - const osgViewer::GraphicsWindow* getGraphicsWindow(void) const; + /** + * @brief Get screen y-coordinate of mouse cursor. + * @return screen y-coordinate of mouse cursor. + */ + int getMouseY(void); + /** + * @brief Handle widget resize event. + * @param width New width of widget. + * @param height New height of widget. + */ virtual void resizeGL(int width, int height); + + /** + * @brief Handle widget paint event. + */ virtual void paintGL(void); + + /** + * @brief This function is a container for user-defined rendering. + * All code to render objects should be in this function. + */ + virtual void display(void); + + /** + * @brief Processes key press events. + * If this handler is reimplemented, it is very important that you call the + * base class implementation. + * @param event Key press event. + */ virtual void keyPressEvent(QKeyEvent* event); + + /** + * @brief Processes key release events. + * If this handler is reimplemented, it is very important that you call the + * base class implementation. + * @param event Key release event. + */ virtual void keyReleaseEvent(QKeyEvent* event); + + /** + * @brief Processes mouse press events. + * If this handler is reimplemented, it is very important that you call the + * base class implementation. + * @param event Mouse press event. + */ virtual void mousePressEvent(QMouseEvent* event); + + /** + * @brief Processes mouse release events. + * If this handler is reimplemented, it is very important that you call the + * base class implementation. + * @param event Mouse release event. + */ virtual void mouseReleaseEvent(QMouseEvent* event); + + /** + * @brief Processes mouse move events. + * If this handler is reimplemented, it is very important that you call the + * base class implementation. + * @param event Mouse move event. + */ virtual void mouseMoveEvent(QMouseEvent* event); + + /** + * @brief Processes mouse wheel events. + * If this handler is reimplemented, it is very important that you call the + * base class implementation. + * @param event Mouse wheel event. + */ virtual void wheelEvent(QWheelEvent* event); + /** + * @brief Converts radians to degrees. + * @param angle Angle in radians. + * @return angle in degrees. + */ float r2d(float angle); + + /** + * @brief Converts degrees to radians. + * @param angle Angle in degrees. + * @return angle in radians. + */ float d2r(float angle); + + /** + * @brief Converts Qt-defined key to OSG-defined key. + * @param key Qt-defined key. + * @return OSG-defined key. + */ osgGA::GUIEventAdapter::KeySymbol convertKey(int key) const; + + /** + * @brief Computes intersection of line and plane. + * @param plane Vector which represents the plane. + * @param line Line representation. + * @return 3D point which lies at the intersection of the line and plane. + */ bool getPlaneLineIntersection(const osg::Vec4d& plane, const osg::LineSegment& line, osg::Vec3d& isect); - osg::ref_ptr root; + osg::ref_ptr root; /**< Root node of scene graph. */ osg::ref_ptr allocentricMap; osg::ref_ptr rollingMap; osg::ref_ptr egocentricMap; osg::ref_ptr robotPosition; osg::ref_ptr robotAttitude; - osg::ref_ptr hudGeode; - osg::ref_ptr hudProjectionMatrix; + osg::ref_ptr hudGeode; /**< A geode which contains renderable HUD objects. */ + osg::ref_ptr hudProjectionMatrix; /**< An orthographic projection matrix for HUD display. */ - DisplayFunc userDisplayFunc; - KeyboardFunc userKeyboardFunc; - MouseFunc userMouseFunc; - MotionFunc userMotionFunc; - void (*userTimerFunc)(void *); + osg::ref_ptr osgGW; /**< A class which manages OSG graphics windows and events. */ - void* userDisplayFuncData; - void* userKeyboardFuncData; - void* userMouseFuncData; - void* userMotionFuncData; - void* userTimerFuncData; + osg::ref_ptr cameraManipulator; /**< Camera manipulator. */ - osg::ref_ptr osgGW; - - QTimer timer; + QTimer timer; /**< Timer which draws graphics based on specified fps. */ struct CameraParams { @@ -143,12 +265,7 @@ protected: float maxClipRange; }; - CameraParams cameraParams; - - int lastMouseX; - int lastMouseY; - - bool _forceRedraw; + CameraParams cameraParams; /**< Struct representing camera parameters. */ }; #endif // Q3DWIDGET_H diff --git a/src/ui/map3D/Q3DWidgetFactory.h b/src/ui/map3D/Q3DWidgetFactory.h index 145cb69895c652d52b133b4546cdb9da380b71a9..4e95b70fd9d0535ac059f0a69986928c7f218e0b 100644 --- a/src/ui/map3D/Q3DWidgetFactory.h +++ b/src/ui/map3D/Q3DWidgetFactory.h @@ -1,3 +1,34 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +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 . + +======================================================================*/ + +/** + * @file + * @brief Definition of the class Q3DWidgetFactory. + * + * @author Lionel Heng + * + */ + #ifndef Q3DWIDGETFACTORY_H #define Q3DWIDGETFACTORY_H @@ -5,9 +36,20 @@ #include "Q3DWidget.h" +/** + * @brief A factory which creates project-specific 3D widgets without + * specifying the exact widget class. + */ + class Q3DWidgetFactory { public: + /** + * @brief Returns a Q3DWidget instance of the appropriate type. + * @param type String specifying the required type name. + * @return A smart pointer to the Q3DWidget instance. + */ + static QPointer get(const std::string& type); }; diff --git a/src/ui/map3D/Texture.cc b/src/ui/map3D/Texture.cc deleted file mode 100644 index 8efaa585873344886380d264dadf11e11f3d89c7..0000000000000000000000000000000000000000 --- a/src/ui/map3D/Texture.cc +++ /dev/null @@ -1,221 +0,0 @@ -/*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 QGROUNDCONTROL PROJECT - -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 . - -======================================================================*/ - -/** - * @file - * @brief Definition of the class Texture. - * - * @author Lionel Heng - * - */ - -#include "Texture.h" - -Texture::Texture() - : _is3D(false) -{ - -} - -const QString& -Texture::getSourceURL(void) const -{ - return sourceURL; -} - -void -Texture::setID(GLuint id) -{ - this->id = id; -} - -void -Texture::sync(const WebImagePtr& image) -{ - state = static_cast(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(imageWidth) - / static_cast(textureWidth); - maxV = static_cast(imageHeight) - / static_cast(textureHeight); - - 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(); - } -} - -void -Texture::draw(float x1, float y1, float x2, float y2, - bool smoothInterpolation) const -{ - draw(x1, y1, x2, y1, x2, y2, x1, y2, smoothInterpolation); -} - -void -Texture::draw(float x1, float y1, float x2, float y2, - float x3, float y3, float x4, float y4, - bool smoothInterpolation) const -{ - if (state == REQUESTED) - { - glBegin(GL_LINE_LOOP); - glColor3f(0.0f, 0.0f, 1.0f); - glVertex2f(x1, y1); - glVertex2f(x2, y2); - glVertex2f(x3, y3); - glVertex2f(x4, y4); - glEnd(); - - return; - } - - glEnable(GL_TEXTURE_2D); - glBindTexture(GL_TEXTURE_2D, id); - - float dx, dy; - if (smoothInterpolation) - { - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); - dx = 1.0f / (2.0f * textureWidth); - dy = 1.0f / (2.0f * textureHeight); - } - else - { - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); - dx = 0.0f; - dy = 0.0f; - } - - glColor3f(1.0f, 1.0f, 1.0f); - if (!_is3D) - { - 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(heightModel.size() - 1); - - for (int32_t i = 0; i < heightModel.size() - 1; ++i) - { - float scaleI = scaleX * static_cast(i); - - float scaleY = - 1.0f / static_cast(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(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(heightModel[i][j])); - glTexCoord2f(dx + (scaleJ + scaleY) * (maxU - dx * 2.0f), - dy + (1.0f - scaleI) * (maxV - dy * 2.0f)); - glVertex3f(nx2, ny2, -static_cast(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(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(heightModel[i + 1][j])); - - glEnd(); - } - } - } - - glDisable(GL_TEXTURE_2D); -} - -bool -Texture::is3D(void) const -{ - return _is3D; -} diff --git a/src/ui/map3D/Texture.h b/src/ui/map3D/Texture.h deleted file mode 100644 index 79dc7355cb158ea4344bba70a6a41b6c685f14ef..0000000000000000000000000000000000000000 --- a/src/ui/map3D/Texture.h +++ /dev/null @@ -1,91 +0,0 @@ -/*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 QGROUNDCONTROL PROJECT - -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 . - -======================================================================*/ - -/** - * @file - * @brief Definition of the class Texture. - * - * @author Lionel Heng - * - */ - -#ifndef TEXTURE_H -#define TEXTURE_H - -#if (defined __APPLE__) & (defined __MACH__) -#include -#else -#include -#endif -#include -#include - -#include "WebImage.h" - -class Texture -{ -public: - Texture(); - - const QString& getSourceURL(void) const; - - void setID(GLuint id); - - void sync(const WebImagePtr& image); - - void draw(float x1, float y1, float x2, float y2, - bool smoothInterpolation) const; - void 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; - GLuint id; - - int32_t textureWidth; - int32_t textureHeight; - - int32_t imageWidth; - int32_t imageHeight; - - bool _is3D; - QVector< QVector > heightModel; - - float maxU; - float maxV; -}; - -typedef QSharedPointer TexturePtr; - -#endif // TEXTURE_H diff --git a/src/ui/map3D/TextureCache.cc b/src/ui/map3D/TextureCache.cc deleted file mode 100644 index 8449f74044c4305ccada6a755ce17fee7bc43cfe..0000000000000000000000000000000000000000 --- a/src/ui/map3D/TextureCache.cc +++ /dev/null @@ -1,114 +0,0 @@ -/*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 QGROUNDCONTROL PROJECT - -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 . - -======================================================================*/ - -/** - * @file - * @brief Definition of the class TextureCache. - * - * @author Lionel Heng - * - */ - -#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); - - 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); - - textures.push_back(t); - } -} - -TexturePtr -TextureCache::get(const QString& tileURL, bool useHeightModel) -{ - QPair p1 = lookup(tileURL, useHeightModel); - if (!p1.first.isNull()) - { - return p1.first; - } - - QPair 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 -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; -} diff --git a/src/ui/map3D/WebImage.cc b/src/ui/map3D/WebImage.cc deleted file mode 100644 index 466d6879542f23cd21e030c3390d850d1966e648..0000000000000000000000000000000000000000 --- a/src/ui/map3D/WebImage.cc +++ /dev/null @@ -1,232 +0,0 @@ -/*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 QGROUNDCONTROL PROJECT - -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 . - -======================================================================*/ - -/** - * @file - * @brief Definition of the class WebImage. - * - * @author Lionel Heng - * - */ - -#include "WebImage.h" - -#include -#include - -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 >& -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(header)); - int32_t width = *(reinterpret_cast(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 scanline; - for (int32_t j = 0; j < width; ++j) - { - int32_t n = *(reinterpret_cast(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; -} diff --git a/src/ui/map3D/WebImage.h b/src/ui/map3D/WebImage.h deleted file mode 100644 index c4dc2a1d31a57ede192c049d735960ac06af0dd6..0000000000000000000000000000000000000000 --- a/src/ui/map3D/WebImage.h +++ /dev/null @@ -1,90 +0,0 @@ -/*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 QGROUNDCONTROL PROJECT - -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 . - -======================================================================*/ - -/** - * @file - * @brief Definition of the class WebImage. - * - * @author Lionel Heng - * - */ - -#ifndef WEBIMAGE_H -#define WEBIMAGE_H - -#include -#include -#include -#include - -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 >& 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 image; - QVector< QVector > heightModel; - uint64_t lastReference; - bool _is3D; - bool syncFlag; -}; - -typedef QSharedPointer WebImagePtr; - -#endif // WEBIMAGE_H diff --git a/src/ui/map3D/WebImageCache.cc b/src/ui/map3D/WebImageCache.cc deleted file mode 100644 index 2203f2141422c44370d7ca71807ea78cdf95675c..0000000000000000000000000000000000000000 --- a/src/ui/map3D/WebImageCache.cc +++ /dev/null @@ -1,191 +0,0 @@ -/*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 QGROUNDCONTROL PROJECT - -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 . - -======================================================================*/ - -/** - * @file - * @brief Definition of the class WebImageCache. - * - * @author Lionel Heng - * - */ - -#include "WebImageCache.h" - -#include -#include - -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 -WebImageCache::lookup(const QString& url, bool useHeightModel) -{ - QPair 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; - } - } -} diff --git a/src/ui/map3D/WebImageCache.h b/src/ui/map3D/WebImageCache.h deleted file mode 100644 index 7b2f1ea6b8163ab1cf165a1a4997cdb0a05fcc41..0000000000000000000000000000000000000000 --- a/src/ui/map3D/WebImageCache.h +++ /dev/null @@ -1,65 +0,0 @@ -/*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 QGROUNDCONTROL PROJECT - -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 . - -======================================================================*/ - -/** - * @file - * @brief Definition of the class WebImageCache. - * - * @author Lionel Heng - * - */ - -#ifndef WEBIMAGECACHE_H -#define WEBIMAGECACHE_H - -#include -#include -#include - -#include "WebImage.h" - -class WebImageCache : public QObject -{ - Q_OBJECT - -public: - WebImageCache(QObject* parent, uint32_t cacheSize); - - QPair lookup(const QString& url, - bool useHeightModel); - - WebImagePtr at(int32_t index) const; - -private Q_SLOTS: - void downloadFinished(QNetworkReply* reply); - -private: - uint32_t cacheSize; - - QVector webImages; - uint64_t currentReference; - - QScopedPointer networkManager; -}; - -#endif // WEBIMAGECACHE_H