Fixed issue with transform from screen to world coordinates.

src/ui/map3D/Pixhawk3DWidget.h

@@ -33,6 +33,7 @@
 #define PIXHAWK3DWIDGET_H
 
 #include <osgText/Text>
+#include <osgEarth/MapNode>
 
 #include "Q3DWidget.h"
 
@@ -66,11 +67,11 @@ public:
 
 public slots:
     void setActiveUAS(UASInterface* uas);
-    void markTarget(void);
 
 private slots:
     void showGrid(int state);
     void showTrail(int state);
+    void showWaypoints(int state);
     void recenterCamera(void);
     void toggleLockCamera(int state);
 
@@ -80,6 +81,7 @@ protected:
 private:
     osg::ref_ptr<osg::Geode> createGrid(void);
     osg::ref_ptr<osg::Geode> createTrail(void);
+    osg::ref_ptr<osgEarth::MapNode> createMap(void);
     osg::ref_ptr<osg::Group> createTarget(void);
     osg::ref_ptr<osg::Group> createWaypoints(void);
 
@@ -91,6 +93,8 @@ private:
     void updateTarget(float robotX, float robotY, float robotZ);
     void updateWaypoints(void);
 
+    void markTarget(void);
+
     double lastRedrawTime;
 
     bool displayGrid;
@@ -98,13 +102,6 @@ private:
     bool displayTarget;
     bool displayWaypoints;
 
-    typedef struct
-    {
-        float x;
-        float y;
-        float z;
-    } Pose3D;
-
     bool lockCamera;
 
     osg::ref_ptr<osg::Vec3Array> trailVertices;
@@ -118,8 +115,11 @@ private:
     osg::ref_ptr<osg::Geode> trailNode;
     osg::ref_ptr<osg::Geometry> trailGeometry;
     osg::ref_ptr<osg::DrawArrays> trailDrawArrays;
+    osg::ref_ptr<osgEarth::MapNode> mapNode;
     osg::ref_ptr<osg::Group> targetNode;
     osg::ref_ptr<osg::Group> waypointsNode;
+
+    QPushButton* targetButton;
 };
 
 #endif // PIXHAWK3DWIDGET_H

src/ui/map3D/Q3DWidget.cc

@@ -262,24 +262,29 @@ Q3DWidget::addTimerFunc(uint32_t msecs, void(*func)(void *),
     QTimer::singleShot(msecs, this, SLOT(userTimer()));
 }
 
-std::pair<float,float>
-Q3DWidget::getGlobalCursorPosition(int32_t mouseX, int32_t mouseY)
+std::pair<double,double>
+Q3DWidget::getGlobalCursorPosition(int32_t mouseX, int32_t mouseY,
+                                   double z)
 {
     osgUtil::LineSegmentIntersector::Intersections intersections;
 
     // normalize cursor position to value between -1 and 1
-    float x = -1.0f + static_cast<float>(2 * mouseX)
-              / static_cast<float>(width());
-    float y = -1.0f + static_cast<float>(2 * (height() - mouseY))
-              / static_cast<float>(height());
+    double x = -1.0f + static_cast<double>(2 * mouseX)
+              / static_cast<double>(width());
+    double y = -1.0f + static_cast<double>(2 * (height() - mouseY))
+              / static_cast<double>(height());
 
-    osg::Matrix pm = getCamera()->getProjectionMatrix();
-    osg::Matrix vm = getCamera()->getViewMatrix();
+    osg::Matrixd m = getCamera()->getViewMatrix()
+                     * getCamera()->getProjectionMatrix();
+    osg::Matrixd invM = osg::Matrixd::inverse(m);
+
+    osg::Vec3d nearPoint = osg::Vec3d(x, y, -1.0) * invM;
+    osg::Vec3d farPoint = osg::Vec3d(x, y, 1.0) * invM;
 
     osg::ref_ptr<osg::LineSegment> line =
-            projectNormalizedXYIntoObjectCoordinates(pm, vm, x, y);
+            new osg::LineSegment(nearPoint, farPoint);
 
-    osg::Plane p(line->start() - line->end(), 0.0);
+    osg::Plane p(osg::Vec3d(0.0, 0.0, 1.0), osg::Vec3d(0.0, 0.0, z));
 
     osg::Vec3d projectedPoint;
     getPlaneLineIntersection(p.asVec4(), *line, projectedPoint);
@@ -618,24 +623,6 @@ Q3DWidget::convertKey(int key) const
     }
 }
 
-osg::ref_ptr<osg::LineSegment>
-Q3DWidget::projectNormalizedXYIntoObjectCoordinates(
-        const osg::Matrix& projectionMatrix,
-        const osg::Matrix& viewMatrix,
-        float x,
-        float y) const
-{
-    osg::Matrix matrix = viewMatrix * projectionMatrix;
-    osg::Matrix inverseVP;
-    inverseVP.invert(matrix);
-
-    osg::Vec3 nearPoint = osg::Vec3(x, y, -1.0f) * inverseVP;
-    osg::Vec3 farPoint = osg::Vec3(x, y, 1.0f) * inverseVP;
-
-    return osg::ref_ptr<osg::LineSegment>(
-            new osg::LineSegment(nearPoint, farPoint));
-}
-
 bool
 Q3DWidget::getPlaneLineIntersection(const osg::Vec4d& plane,
                                     const osg::LineSegment& line,

src/ui/map3D/Q3DWidget.h

@@ -73,8 +73,9 @@ public:
     void addTimerFunc(uint msecs, void(*func)(void *),
                       void* clientData);
 
-    std::pair<float,float> getGlobalCursorPosition(int32_t mouseX,
-                                                   int32_t mouseY);
+    std::pair<double,double> getGlobalCursorPosition(int32_t mouseX,
+                                                     int32_t mouseY,
+                                                     double z);
 
 protected slots:
     void redraw(void);
@@ -104,11 +105,6 @@ protected:
     float r2d(float angle);
     float d2r(float angle);
     osgGA::GUIEventAdapter::KeySymbol convertKey(int key) const;
-    osg::ref_ptr<osg::LineSegment> projectNormalizedXYIntoObjectCoordinates(
-            const osg::Matrix& projectionMatrix,
-            const osg::Matrix& viewMatrix,
-            float x,
-            float y) const;
     bool getPlaneLineIntersection(const osg::Vec4d& plane,
                                   const osg::LineSegment& line,
                                   osg::Vec3d& isect);

src/ui/map3D/Q3DWidgetFactory.cc

+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+
+This file is part of the QGROUNDCONTROL project
+
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ *   @brief Definition of the class Q3DWidgetFactory.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
 #include "Q3DWidgetFactory.h"
 
 #include "Pixhawk3DWidget.h"