From f00bbdfa9ae37f5192ed0008d5419256ca25b08d Mon Sep 17 00:00:00 2001
From: hengli <hengli@student.ethz.ch>
Date: Mon, 25 Oct 2010 11:12:19 +0200
Subject: [PATCH] Fixed improper z-height rendering when camera is unlocked.

---
 src/comm/MAVLinkSimulationLink.cc | 25 +++++++++++--------------
 src/ui/map3D/QMap3DWidget.cc      | 22 +++++++++++-----------
 src/ui/map3D/QMap3DWidget.h       |  6 +++---
 3 files changed, 25 insertions(+), 28 deletions(-)

diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc
index b5e2096a5..e55d7d871 100644
--- a/src/comm/MAVLinkSimulationLink.cc
+++ b/src/comm/MAVLinkSimulationLink.cc
@@ -100,9 +100,10 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile
     mavlinkLogFile = new QFile(MAVLinkProtocol::getLogfileName());
     mavlinkLogFile->open(QIODevice::ReadOnly);
 
-    x = 0;
-    y = 0;
-    z = 0;
+    // position at Pixhawk lab @ ETHZ
+    x = 5247273.0f;
+    y = 465955.0f;
+    z = -0.2f;
     yaw = 0;
 }
 
@@ -381,17 +382,13 @@ void MAVLinkSimulationLink::mainloop()
         y += sin(QGC::groundTimeUsecs()) * 0.05f;
         z += sin(QGC::groundTimeUsecs()) * 0.009f;
 
-        x = (x > 5.0f) ? 5.0f : x;
-        y = (y > 5.0f) ? 5.0f : y;
-        z = (z > 3.0f) ? 3.0f : z;
-
-        x = (x < -5.0f) ? -5.0f : x;
-        y = (y < -5.0f) ? -5.0f : y;
-        z = (z < -3.0f) ? -3.0f : z;
-
-        // position at Pixhawk lab @ ETHZ
-        x += 5247273.0f;
-        y += 465955.0f;
+//        x = (x > 5.0f) ? 5.0f : x;
+//        y = (y > 5.0f) ? 5.0f : y;
+//        z = (z > 3.0f) ? 3.0f : z;
+//
+//        x = (x < -5.0f) ? -5.0f : x;
+//        y = (y < -5.0f) ? -5.0f : y;
+//        z = (z < -3.0f) ? -3.0f : z;
 
         // Send back new setpoint
         mavlink_message_t ret;
diff --git a/src/ui/map3D/QMap3DWidget.cc b/src/ui/map3D/QMap3DWidget.cc
index e8b7b679e..830c7b8d1 100644
--- a/src/ui/map3D/QMap3DWidget.cc
+++ b/src/ui/map3D/QMap3DWidget.cc
@@ -170,7 +170,7 @@ QMap3DWidget::displayHandler(void)
         robotYaw = uas->getYaw();
     }
 
-    if (updateLastUnlockedPose)
+    if (updateLastUnlockedPose && uas != NULL)
     {
         lastUnlockedPose.x = robotX;
         lastUnlockedPose.y = robotY;
@@ -209,7 +209,7 @@ QMap3DWidget::displayHandler(void)
 
     if (displayGrid)
     {
-        drawGrid();
+        drawGrid(-camOffset.x, -camOffset.y, robotZ);
     }
 
     if (displayTrail)
@@ -229,7 +229,7 @@ QMap3DWidget::displayHandler(void)
 
     if (displayImagery)
     {
-        drawImagery(robotX, robotY, "32T", true);
+        drawImagery(robotX, robotY, robotZ, "32T", true);
     }
 
     glPopMatrix();
@@ -624,7 +624,7 @@ QMap3DWidget::drawPlatform(float roll, float pitch, float yaw) const
 }
 
 void
-QMap3DWidget::drawGrid(void) const
+QMap3DWidget::drawGrid(float x, float y, float z) const
 {
     float radius = 10.0f;
     float resolution = 0.25f;
@@ -645,10 +645,10 @@ QMap3DWidget::drawGrid(void) const
         }
 
         glBegin(GL_LINES);
-        glVertex3f(i, -radius, 0.0f);
-        glVertex3f(i, radius, 0.0f);
-        glVertex3f(-radius, i, 0.0f);
-        glVertex3f(radius, i, 0.0f);
+        glVertex3f(x + i, y - radius, -z);
+        glVertex3f(x + i, y + radius, -z);
+        glVertex3f(x - radius, y + i, -z);
+        glVertex3f(x + radius, y + i, -z);
         glEnd();
     }
 
@@ -656,13 +656,13 @@ QMap3DWidget::drawGrid(void) const
 }
 
 void
-QMap3DWidget::drawImagery(double originX, double originY, const QString& zone,
-                          bool prefetch) const
+QMap3DWidget::drawImagery(double originX, double originY, double originZ,
+                          const QString& zone, bool prefetch) const
 {
     glPushMatrix();
     glEnable(GL_BLEND);
 
-    glTranslatef(0, 0, 0.1);
+    glTranslatef(0, 0, -originZ);
 
     double viewingRadius = cameraPose.distance / 4000.0 * 3000.0;
     if (viewingRadius < 100.0)
diff --git a/src/ui/map3D/QMap3DWidget.h b/src/ui/map3D/QMap3DWidget.h
index 0ab296182..1b547b8ce 100644
--- a/src/ui/map3D/QMap3DWidget.h
+++ b/src/ui/map3D/QMap3DWidget.h
@@ -86,9 +86,9 @@ protected:
 
 private:
     void drawPlatform(float roll, float pitch, float yaw) const;
-    void drawGrid(void) const;
-    void drawImagery(double originX, double originY, const QString& zone,
-                     bool prefetch) const;
+    void drawGrid(float x, float y, float z) const;
+    void drawImagery(double originX, double originY, double originZ,
+                     const QString& zone, bool prefetch) const;
     void drawTrail(float x, float y, float z);
     void drawTarget(float x, float y, float z) const;
 
-- 
2.22.0