Fixed bugs in waypoint management for 3D view.

src/ui/map3D/Imagery.cc

@@ -118,7 +118,7 @@ Imagery::prefetch2D(double windowWidth, double windowHeight,
 void
 Imagery::draw2D(double windowWidth, double windowHeight,
                 double zoom, double xOrigin, double yOrigin,
-                double xOffset, double yOffset,
+                double xOffset, double yOffset, double zOffset,
                 const QString& utmZone)
 {
     if (getNumDrawables() > 0)
@@ -176,6 +176,7 @@ Imagery::draw2D(double windowWidth, double windowHeight,
                                     y2 - yOffset, x2 - xOffset,
                                     y3 - yOffset, x3 - xOffset,
                                     y4 - yOffset, x4 - xOffset,
+                                    zOffset,
                                     true));
             }
         }
@@ -214,7 +215,7 @@ Imagery::prefetch3D(double radius, double tileResolution,
 void
 Imagery::draw3D(double radius, double tileResolution,
                 double xOrigin, double yOrigin,
-                double xOffset, double yOffset,
+                double xOffset, double yOffset, double zOffset,
                 const QString& utmZone)
 {
     if (getNumDrawables() > 0)
@@ -252,6 +253,7 @@ Imagery::draw3D(double radius, double tileResolution,
                                     y2 - yOffset, x2 - xOffset,
                                     y3 - yOffset, x3 - xOffset,
                                     y4 - yOffset, x4 - xOffset,
+                                    zOffset,
                                     true));
             }
         }

src/ui/map3D/Imagery.h

@@ -60,7 +60,7 @@ public:
                     const QString& utmZone);
     void draw2D(double windowWidth, double windowHeight,
                 double zoom, double xOrigin, double yOrigin,
-                double xOffset, double yOffset,
+                double xOffset, double yOffset, double zOffset,
                 const QString& utmZone);
 
     void prefetch3D(double radius, double tileResolution,
@@ -68,7 +68,7 @@ public:
                     const QString& utmZone);
     void draw3D(double radius, double tileResolution,
                 double xOrigin, double yOrigin,
-                double xOffset, double yOffset,
+                double xOffset, double yOffset, double zOffset,
                 const QString& utmZone);
 
     bool update(void);

src/ui/map3D/Pixhawk3DWidget.cc

@@ -289,7 +289,7 @@ Pixhawk3DWidget::setWaypointAltitude(void)
     {
         const QVector<Waypoint *> waypoints =
                 uas->getWaypointManager().getWaypointList();
-        waypoints.at(selectedWpIndex)->setZ(0.0);
+//        waypoints.at(selectedWpIndex)->setZ(0.0);
     }
 }
 
@@ -303,11 +303,12 @@ Pixhawk3DWidget::clearAllWaypoints(void)
         std::pair<double,double> cursorWorldCoords =
                 getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
 
-        Waypoint* wp = new Waypoint(0,
-                                    cursorWorldCoords.first,
-                                    cursorWorldCoords.second,
-                                    -altitude);
-        uas->getWaypointManager().addWaypoint(wp);
+        const QVector<Waypoint *> waypoints =
+                uas->getWaypointManager().getWaypointList();
+        for (int i = waypoints.size() - 1; i >= 0; --i)
+        {
+            uas->getWaypointManager().removeWaypoint(i);
+        }
     }
 }
 
@@ -485,7 +486,7 @@ Pixhawk3DWidget::display(void)
 
     if (displayImagery)
     {
-        updateImagery(robotX, robotY, utmZone);
+        updateImagery(robotX, robotY, robotZ, utmZone);
     }
 
     if (displayTarget)
@@ -888,7 +889,7 @@ Pixhawk3DWidget::updateTrail(double robotX, double robotY, double robotZ)
 }
 
 void
-Pixhawk3DWidget::updateImagery(double originX, double originY,
+Pixhawk3DWidget::updateImagery(double originX, double originY, double originZ,
                                const QString& zone)
 {
     if (mapNode->getImageryType() == Imagery::BLANK_MAP)
@@ -938,6 +939,7 @@ Pixhawk3DWidget::updateImagery(double originX, double originY,
                     cameraManipulator->getCenter().x(),
                     originX,
                     originY,
+                    originZ,
                     zone);
 
     // prefetch map tiles
@@ -1025,7 +1027,7 @@ Pixhawk3DWidget::updateWaypoints(void)
 
             osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
             osg::ref_ptr<osg::Cylinder> cylinder =
-                    new osg::Cylinder(osg::Vec3d(0.0, 0.0, 0.0),
+                    new osg::Cylinder(osg::Vec3d(0.0, 0.0, - wp->getZ() / 2.0),
                                       wp->getOrbit(),
                                       fabs(wp->getZ()));
 
@@ -1044,12 +1046,40 @@ Pixhawk3DWidget::updateWaypoints(void)
             osg::ref_ptr<osg::Geode> geode = new osg::Geode;
             geode->addDrawable(sd);
 
+            char wpLabel[10];
+            sprintf(wpLabel, "wp%d", i);
+            geode->setName(wpLabel);
+
+            if (i < list.size() - 1)
+            {
+                osg::ref_ptr<osg::Geometry> geometry = new osg::Geometry;
+                osg::ref_ptr<osg::Vec3dArray> vertices = new osg::Vec3dArray;
+                vertices->push_back(osg::Vec3d(0.0, 0.0, -wp->getZ()));
+                vertices->push_back(osg::Vec3d(list.at(i+1)->getY() - wp->getY(),
+                                               list.at(i+1)->getX() - wp->getX(),
+                                               -list.at(i+1)->getZ()));
+                geometry->setVertexArray(vertices);
+
+                osg::ref_ptr<osg::Vec4Array> colors = new osg::Vec4Array;
+                colors->push_back(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f));
+                geometry->setColorArray(colors);
+                geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
+
+                geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, 2));
+
+                osg::ref_ptr<osg::LineWidth> linewidth(new osg::LineWidth());
+                linewidth->setWidth(2.0f);
+                geometry->getOrCreateStateSet()->setAttributeAndModes(linewidth, osg::StateAttribute::ON);
+
+                geode->addDrawable(geometry);
+            }
+
             osg::ref_ptr<osg::PositionAttitudeTransform> pat =
                     new osg::PositionAttitudeTransform;
 
             pat->setPosition(osg::Vec3d(wp->getY() - robotY,
                                         wp->getX() - robotX,
-                                        wp->getZ() / 2.0 - robotZ));
+                                        robotZ));
 
             waypointsNode->addChild(pat);
             pat->addChild(geode);
@@ -1282,12 +1312,10 @@ Pixhawk3DWidget::findWaypoint(int mouseX, int mouseY)
                 for (uint i = 0 ; i < it->nodePath.size(); ++i)
                 {
                     std::string nodeName = it->nodePath[i]->getName();
-                    if (nodeName.c_str()[0] == 'w' && nodeName.c_str()[1] == 'p')
+                    if (nodeName.substr(0, 2).compare("wp") == 0)
                     {
-                        char wpNum[5];
-                        wpNum[0] = nodeName[2];
-                        wpNum[1] = nodeName[3];
-                        return atoi(wpNum);
+                        qDebug() << nodeName.c_str() << "Got!!";
+                        return atoi(nodeName.substr(2).c_str());
                     }
                 }
             }
@@ -1312,15 +1340,13 @@ Pixhawk3DWidget::showEditWaypointMenu(const QPoint &cursorPos)
     QMenu menu;
 
     QString text;
-    text.append("Move waypoint %1").arg(QString::number(selectedWpIndex));
+    text = QString("Move waypoint %1").arg(QString::number(selectedWpIndex));
     menu.addAction(text, this, SLOT(moveWaypoint()));
 
-    text.clear();
-    text.append("Change altitude of waypoint %1").arg(QString::number(selectedWpIndex));
+    text = QString("Change altitude of waypoint %1").arg(QString::number(selectedWpIndex));
     menu.addAction(text, this, SLOT(setWaypointAltitude()));
 
-    text.clear();
-    text.append("Delete waypoint %1").arg(QString::number(selectedWpIndex));
+    text = QString("Delete waypoint %1").arg(QString::number(selectedWpIndex));
     menu.addAction(text, this, SLOT(deleteWaypoint()));
 
     menu.addAction("Clear all waypoints", this, SLOT(clearAllWaypoints()));

src/ui/map3D/Pixhawk3DWidget.h

@@ -98,7 +98,8 @@ private:
     void updateHUD(double robotX, double robotY, double robotZ,
                    double robotRoll, double robotPitch, double robotYaw);
     void updateTrail(double robotX, double robotY, double robotZ);
-    void updateImagery(double originX, double originY, const QString& zone);
+    void updateImagery(double originX, double originY, double originZ,
+                       const QString& zone);
     void updateTarget(void);
     void updateWaypoints(void);
 #ifdef QGC_LIBFREENECT_ENABLED

src/ui/map3D/Texture.cc

@@ -113,22 +113,24 @@ Texture::sync(const WebImagePtr& image)
 
 osg::ref_ptr<osg::Geometry>
 Texture::draw(double x1, double y1, double x2, double y2,
+              double z,
               bool smoothInterpolation) const
 {
-    return draw(x1, y1, x2, y1, x2, y2, x1, y2, smoothInterpolation);
+    return draw(x1, y1, x2, y1, x2, y2, x1, y2, z, smoothInterpolation);
 }
 
 osg::ref_ptr<osg::Geometry>
 Texture::draw(double x1, double y1, double x2, double y2,
               double x3, double y3, double x4, double y4,
+              double z,
               bool smoothInterpolation) const
 {
     osg::Vec3dArray* vertices =
             static_cast<osg::Vec3dArray*>(geometry->getVertexArray());
-    (*vertices)[0].set(x1, y1, -0.1);
-    (*vertices)[1].set(x2, y2, -0.1);
-    (*vertices)[2].set(x3, y3, -0.1);
-    (*vertices)[3].set(x4, y4, -0.1);
+    (*vertices)[0].set(x1, y1, z - 0.1);
+    (*vertices)[1].set(x2, y2, z - 0.1);
+    (*vertices)[2].set(x3, y3, z - 0.1);
+    (*vertices)[3].set(x4, y4, z - 0.1);
 
     osg::DrawArrays* drawarrays =
             static_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));

src/ui/map3D/Texture.h

@@ -52,9 +52,11 @@ public:
     void sync(const WebImagePtr& image);
 
     osg::ref_ptr<osg::Geometry> draw(double x1, double y1, double x2, double y2,
+                                     double z,
                                      bool smoothInterpolation) const;
     osg::ref_ptr<osg::Geometry> draw(double x1, double y1, double x2, double y2,
                                      double x3, double y3, double x4, double y4,
+                                     double z,
                                      bool smoothInterpolation) const;
 
 private: