From 97ea31898d06dafa23855e8b327c610f645786fc Mon Sep 17 00:00:00 2001 From: Lionel Heng Date: Tue, 19 Oct 2010 22:17:22 +0200 Subject: [PATCH] Fixed all known bugs in 3D imagery. --- src/ui/map3D/Imagery.cc | 6 ++- src/ui/map3D/Q3DWidget.h | 1 - src/ui/map3D/QMap3DWidget.cc | 93 ++++++++++++++++++++++++++++++------ src/ui/map3D/QMap3DWidget.h | 4 ++ 4 files changed, 86 insertions(+), 18 deletions(-) diff --git a/src/ui/map3D/Imagery.cc b/src/ui/map3D/Imagery.cc index 001506b00..7a768f1db 100644 --- a/src/ui/map3D/Imagery.cc +++ b/src/ui/map3D/Imagery.cc @@ -6,6 +6,8 @@ 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)) { @@ -261,7 +263,7 @@ Imagery::imageBounds(int32_t x, int32_t y, double imageResolution, double& x1, double& y1, double& x2, double& y2, double& x3, double& y3, double& x4, double& y4) { - int32_t zoomLevel = 19 - static_cast(rint(log2(imageResolution))); + int32_t zoomLevel = MAX_ZOOM_LEVEL - static_cast(rint(log2(imageResolution))); int32_t numTiles = static_cast(exp2(static_cast(zoomLevel))); double lon1 = 360.0 * (static_cast(x) @@ -306,7 +308,7 @@ Imagery::UTMtoTile(double northing, double easting, const QString& utmZone, UTMtoLL(northing, easting, utmZone, latitude, longitude); - zoomLevel = 19 - static_cast(rint(log2(imageResolution))); + zoomLevel = MAX_ZOOM_LEVEL - static_cast(rint(log2(imageResolution))); int32_t numTiles = static_cast(exp2(static_cast(zoomLevel))); double x = longitude / 180.0; diff --git a/src/ui/map3D/Q3DWidget.h b/src/ui/map3D/Q3DWidget.h index 01193d4c4..5658c64a5 100644 --- a/src/ui/map3D/Q3DWidget.h +++ b/src/ui/map3D/Q3DWidget.h @@ -178,7 +178,6 @@ protected: GLUquadricObj* quadObj; -private: // QGLWidget events void initializeGL(void); void paintGL(void); diff --git a/src/ui/map3D/QMap3DWidget.cc b/src/ui/map3D/QMap3DWidget.cc index b926b5e32..831afc85a 100644 --- a/src/ui/map3D/QMap3DWidget.cc +++ b/src/ui/map3D/QMap3DWidget.cc @@ -56,7 +56,7 @@ QMap3DWidget::QMap3DWidget(QWidget* parent) setFocusPolicy(Qt::StrongFocus); initialize(10, 10, 1000, 900, 15.0f); - setCameraParams(0.05f, 0.5f, 0.01f, 0.5f, 30.0f, 0.01f, 400.0f); + setCameraParams(0.05f, 0.5f, 0.01f, 0.5f, 30.0f, 0.01f, 1000000.0f); setDisplayFunc(display, this); setMouseFunc(mouse, this); @@ -93,7 +93,7 @@ QMap3DWidget::buildLayout(void) QLabel* imageryLabel = new QLabel(this); imageryLabel->setText("Imagery"); - QComboBox* imageryComboBox = new QComboBox(this); + imageryComboBox = new QComboBox(this); imageryComboBox->addItem("None"); imageryComboBox->addItem("Map (Google)"); imageryComboBox->addItem("Satellite (Google)"); @@ -231,6 +231,8 @@ QMap3DWidget::displayHandler(void) // switch to 2D setDisplayMode2D(); + drawLegend(); + // display pose information glColor4f(0.0f, 0.0f, 0.0f, 0.5f); glBegin(GL_POLYGON); @@ -333,6 +335,55 @@ void QMap3DWidget::drawWaypoints() } } +void +QMap3DWidget::drawLegend(void) +{ + // draw marker outlines + glColor3f(1.0f, 1.0f, 1.0f); + glLineWidth(3.0f); + glBegin(GL_LINES); + glVertex2f(20.0f, 60.0f); + glVertex2f(20.0f, 80.0f); + glVertex2f(20.0f, 70.0f); + glVertex2f(100.0f, 70.0f); + glVertex2f(100.0f, 60.0f); + glVertex2f(100.0f, 80.0f); + glEnd(); + + // draw markers + glColor3f(0.0f, 0.0f, 0.0f); + glLineWidth(1.5f); + glBegin(GL_LINES); + glVertex2f(20.0f, 60.0f); + glVertex2f(20.0f, 80.0f); + glVertex2f(20.0f, 70.0f); + glVertex2f(100.0f, 70.0f); + glVertex2f(100.0f, 60.0f); + glVertex2f(100.0f, 80.0f); + glEnd(); + + float f = windowHeight / 2.0f / tanf(d2r(cameraParams.cameraFov / 2.0f)); + float dist = cameraPose.distance / f * 80.0f; + + QPainter painter; + painter.begin(this); + painter.setRenderHint(QPainter::Antialiasing, true); + painter.setRenderHint(QPainter::HighQualityAntialiasing, true); + + QColor rgb(255, 255, 255); + if (imageryComboBox->currentText().compare("Map (Google)") == 0) + { + rgb.setRgb(0, 0, 0); + } + + paintText(QString("%1 m").arg(dist, 0, 'f', 2), + rgb, + 10, + 25, + getWindowHeight() - 65, + &painter); +} + void QMap3DWidget::paintText(QString text, QColor color, float fontSize, float refX, float refY, QPainter* painter) { QPen prevPen = painter->pen(); @@ -450,8 +501,11 @@ QMap3DWidget::markTarget(void) displayTarget = true; - if (uas) uas->setTargetPosition(targetPosition.x, targetPosition.y, - targetPosition.z, 0.0f); + if (uas) + { + uas->setTargetPosition(targetPosition.x, targetPosition.y, + targetPosition.z, 0.0f); + } } void @@ -533,9 +587,9 @@ QMap3DWidget::drawPlatform(float roll, float pitch, float yaw) { glPushMatrix(); - glRotatef((yaw*180.0f)/M_PI, 0.0f, 0.0f, 1.0f); - glRotatef((pitch*180.0f)/M_PI, 0.0f, 1.0f, 0.0f); - glRotatef((roll*180.0f)/M_PI, 1.0f, 0.0f, 0.0f); + glRotatef(yaw * 180.0f / M_PI, 0.0f, 0.0f, 1.0f); + glRotatef(pitch * 180.0f / M_PI, 0.0f, 1.0f, 0.0f); + glRotatef(roll * 180.0f / M_PI, 1.0f, 0.0f, 0.0f); glLineWidth(3.0f); @@ -606,16 +660,24 @@ QMap3DWidget::drawImagery(double originX, double originY, const QString& zone, glTranslatef(0, 0, 0.1); - CameraPose camPose = getCameraPose(); - double viewingRadius = camPose.distance / 4000.0 * 3000.0; + double viewingRadius = cameraPose.distance / 4000.0 * 3000.0; if (viewingRadius < 100.0) { viewingRadius = 100.0; } - double minResolution = 0.125; - double centerResolution = camPose.distance / 160.0; - double maxResolution = 2.0; + double minResolution = 0.25; + double centerResolution = cameraPose.distance / 100.0; + double maxResolution = 1048576.0; + + if (imageryComboBox->currentText().compare("Map (Google)") == 0) + { + minResolution = 0.25; + } + else if (imageryComboBox->currentText().compare("Satellite (Google)") == 0) + { + minResolution = 0.5; + } double resolution = minResolution; while (resolution * 2.0 < centerResolution) @@ -627,7 +689,8 @@ QMap3DWidget::drawImagery(double originX, double originY, const QString& zone, resolution = maxResolution; } - imagery->draw3D(viewingRadius, resolution, originX, originY, camPose.xOffset, camPose.yOffset, zone); + imagery->draw3D(viewingRadius, resolution, originX, originY, + cameraPose.xOffset, cameraPose.yOffset, zone); if (prefetch) { @@ -635,13 +698,13 @@ QMap3DWidget::drawImagery(double originX, double originY, const QString& zone, { imagery->prefetch3D(viewingRadius / 2.0, resolution / 2.0, originX, originY, - camPose.xOffset, camPose.yOffset, zone); + cameraPose.xOffset, cameraPose.yOffset, zone); } if (resolution * 2.0 <= maxResolution) { imagery->prefetch3D(viewingRadius * 2.0, resolution * 2.0, originX, originY, - camPose.xOffset, camPose.yOffset, zone); + cameraPose.xOffset, cameraPose.yOffset, zone); } } diff --git a/src/ui/map3D/QMap3DWidget.h b/src/ui/map3D/QMap3DWidget.h index 038f1b1a4..e0d2a9d7f 100644 --- a/src/ui/map3D/QMap3DWidget.h +++ b/src/ui/map3D/QMap3DWidget.h @@ -91,6 +91,8 @@ private: void drawTrail(float x, float y, float z); void drawTarget(float x, float y, float z); + void drawLegend(void); + double lastRedrawTime; bool displayGrid; @@ -117,6 +119,8 @@ private: QScopedPointer cheetahModel; QScopedPointer imagery; + + QComboBox* imageryComboBox; }; #endif // QMAP3DWIDGET_H -- 2.22.0