diff --git a/src/ui/map3D/Imagery.cc b/src/ui/map3D/Imagery.cc index e0ff4752c1b7e824b9a1fabe7932cf2064763176..084e0892c071173e5ed195cd9e75919608dfaecd 100644 --- a/src/ui/map3D/Imagery.cc +++ b/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)); } } diff --git a/src/ui/map3D/Imagery.h b/src/ui/map3D/Imagery.h index b20f846ab28de03049b3492e999a3a42801e3514..381231235659179adaefe9a38b886ecd6ce5a723 100644 --- a/src/ui/map3D/Imagery.h +++ b/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); diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index 22000c811bf5676d663221ca1e325dabdaeb4b46..84afb023a93a00c5827c3545b3c0570d8e63996b 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/src/ui/map3D/Pixhawk3DWidget.cc @@ -289,7 +289,7 @@ Pixhawk3DWidget::setWaypointAltitude(void) { const QVector 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 cursorWorldCoords = getGlobalCursorPosition(getMouseX(), getMouseY(), altitude); - Waypoint* wp = new Waypoint(0, - cursorWorldCoords.first, - cursorWorldCoords.second, - -altitude); - uas->getWaypointManager().addWaypoint(wp); + const QVector 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 sd = new osg::ShapeDrawable; osg::ref_ptr 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 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 geometry = new osg::Geometry; + osg::ref_ptr 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 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 linewidth(new osg::LineWidth()); + linewidth->setWidth(2.0f); + geometry->getOrCreateStateSet()->setAttributeAndModes(linewidth, osg::StateAttribute::ON); + + geode->addDrawable(geometry); + } + osg::ref_ptr 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())); diff --git a/src/ui/map3D/Pixhawk3DWidget.h b/src/ui/map3D/Pixhawk3DWidget.h index 5b413817977200b8e955ff950cc2be3a3c2b13d2..21c01bea82b7d8309048ecbc925de9168832ddcb 100644 --- a/src/ui/map3D/Pixhawk3DWidget.h +++ b/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 diff --git a/src/ui/map3D/Texture.cc b/src/ui/map3D/Texture.cc index 63b6c7a6f1dba11414e8e64d26251d7be6ec72d0..258ceb17f990e2621765ae55e0ec82e0192f6f57 100644 --- a/src/ui/map3D/Texture.cc +++ b/src/ui/map3D/Texture.cc @@ -113,22 +113,24 @@ Texture::sync(const WebImagePtr& image) osg::ref_ptr 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 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(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(geometry->getPrimitiveSet(0)); diff --git a/src/ui/map3D/Texture.h b/src/ui/map3D/Texture.h index 095fb13f78dde5e0c20e749de85c795449cfb8ad..8bf82ab3b677a8e1a29198d8b1f60f5b4f1fda96 100644 --- a/src/ui/map3D/Texture.h +++ b/src/ui/map3D/Texture.h @@ -52,9 +52,11 @@ public: void sync(const WebImagePtr& image); osg::ref_ptr draw(double x1, double y1, double x2, double y2, + double z, bool smoothInterpolation) const; osg::ref_ptr draw(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4, + double z, bool smoothInterpolation) const; private: