Fixed camera control issue. No known outstanding issues.

qgroundcontrol.pro

@@ -223,7 +223,8 @@ HEADERS += src/MG.h \
     src/ui/map3D/PixhawkCheetahGeode.h \
     src/comm/QGCMAVLink.h \
     src/ui/map3D/Pixhawk3DWidget.h \
-    src/ui/map3D/Q3DWidgetFactory.h
+    src/ui/map3D/Q3DWidgetFactory.h \
+    src/ui/map3D/GCManipulator.h
 SOURCES += src/main.cc \
     src/Core.cc \
     src/uas/UASManager.cc \
@@ -297,7 +298,8 @@ SOURCES += src/main.cc \
     src/ui/map3D/Q3DWidget.cc \
     src/ui/map3D/PixhawkCheetahGeode.cc \
     src/ui/map3D/Pixhawk3DWidget.cc \
-    src/ui/map3D/Q3DWidgetFactory.cc
+    src/ui/map3D/Q3DWidgetFactory.cc \
+    src/ui/map3D/GCManipulator.cc
 RESOURCES = mavground.qrc
 
 # Include RT-LAB Library

src/ui/map3D/GCManipulator.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 GCManipulator.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
+#include "GCManipulator.h"
+
+GCManipulator::GCManipulator()
+{
+    _moveSensitivity = 0.05;
+    _minZoomRange = 2.0;
+}
+
+void
+GCManipulator::setMinZoomRange(double minZoomRange)
+{
+    _minZoomRange = minZoomRange;
+}
+
+bool
+GCManipulator::handle(const osgGA::GUIEventAdapter& ea,
+                      osgGA::GUIActionAdapter& us)
+{
+    using namespace osgGA;
+
+    switch (ea.getEventType())
+    {
+    case GUIEventAdapter::PUSH:
+        {
+            flushMouseEventStack();
+            addMouseEvent(ea);
+            if (calcMovement())
+            {
+                us.requestRedraw();
+            }
+            us.requestContinuousUpdate(false);
+            _thrown = false;
+            return true;
+        }
+
+    case GUIEventAdapter::RELEASE:
+        {
+            if (ea.getButtonMask() == 0)
+            {
+                if (isMouseMoving())
+                {
+                    if (calcMovement())
+                    {
+                        us.requestRedraw();
+                        us.requestContinuousUpdate(true);
+                        _thrown = true;
+                    }
+                }
+                else
+                {
+                    flushMouseEventStack();
+                    addMouseEvent(ea);
+                    if (calcMovement())
+                    {
+                        us.requestRedraw();
+                    }
+                    us.requestContinuousUpdate(false);
+                    _thrown = false;
+                }
+
+            }
+            else
+            {
+                flushMouseEventStack();
+                addMouseEvent(ea);
+                if (calcMovement())
+                {
+                    us.requestRedraw();
+                }
+                us.requestContinuousUpdate(false);
+                _thrown = false;
+            }
+            return true;
+        }
+
+    case GUIEventAdapter::DRAG:
+        {
+            addMouseEvent(ea);
+            if (calcMovement())
+            {
+                us.requestRedraw();
+            }
+            us.requestContinuousUpdate(false);
+            _thrown = false;
+            return true;
+        }
+
+    case GUIEventAdapter::SCROLL:
+        {
+            // zoom model
+            float scale = 1.0f;
+
+            if (ea.getScrollingMotion() == GUIEventAdapter::SCROLL_UP)
+            {
+                scale -= 0.1f;
+            }
+            else
+            {
+                scale += 0.1f;
+            }
+            if (_distance * scale > _minZoomRange)
+            {
+
+                _distance *= scale;
+
+            }
+
+            return true;
+        }
+
+    case GUIEventAdapter::KEYDOWN:
+        // pan model
+        switch (ea.getKey())
+        {
+        case GUIEventAdapter::KEY_Space:
+            {
+                flushMouseEventStack();
+                _thrown = false;
+                home(ea,us);
+                us.requestRedraw();
+                us.requestContinuousUpdate(false);
+                return true;
+            }
+        case GUIEventAdapter::KEY_Left:
+            {
+                float scale = -_moveSensitivity * _distance;
+
+                osg::Matrix rotation_matrix;
+                rotation_matrix.makeRotate(_rotation);
+
+                osg::Vec3 dv(scale, 0.0f, 0.0f);
+
+                _center += dv * rotation_matrix;
+
+                return true;
+            }
+        case GUIEventAdapter::KEY_Right:
+            {
+                float scale = _moveSensitivity * _distance;
+
+                osg::Matrix rotation_matrix;
+                rotation_matrix.makeRotate(_rotation);
+
+                osg::Vec3 dv(scale, 0.0f, 0.0f);
+
+                _center += dv * rotation_matrix;
+
+                return true;
+            }
+        case GUIEventAdapter::KEY_Up:
+            {
+                float scale = _moveSensitivity * _distance;
+
+                osg::Matrix rotation_matrix;
+                rotation_matrix.makeRotate(_rotation);
+
+                osg::Vec3 dv(0.0f, scale, 0.0f);
+
+                _center += dv * rotation_matrix;
+
+                return true;
+            }
+        case GUIEventAdapter::KEY_Down:
+            {
+                float scale = -_moveSensitivity * _distance;
+
+                osg::Matrix rotation_matrix;
+                rotation_matrix.makeRotate(_rotation);
+
+                osg::Vec3 dv(0.0f, scale, 0.0f);
+
+                _center += dv * rotation_matrix;
+
+                return true;
+            }
+            return false;
+        }
+
+    case GUIEventAdapter::FRAME:
+        if (_thrown)
+        {
+            if (calcMovement())
+            {
+                us.requestRedraw();
+            }
+        }
+        return false;
+
+    default:
+        return false;
+    }
+}
+
+
+bool
+GCManipulator::calcMovement()
+{
+    using namespace osgGA;
+
+    // return if less then two events have been added.
+    if (_ga_t0.get() == NULL || _ga_t1.get() == NULL)
+    {
+        return false;
+    }
+
+    float dx = _ga_t0->getXnormalized() - _ga_t1->getXnormalized();
+    float dy = _ga_t0->getYnormalized() - _ga_t1->getYnormalized();
+
+    // return if there is no movement.
+    if (dx == 0.0f && dy == 0.0f)
+    {
+        return false;
+    }
+
+    unsigned int buttonMask = _ga_t1->getButtonMask();
+    if (buttonMask == GUIEventAdapter::LEFT_MOUSE_BUTTON)
+    {
+        // rotate camera
+        osg::Vec3 axis;
+        float angle;
+
+        float px0 = _ga_t0->getXnormalized();
+        float py0 = _ga_t0->getYnormalized();
+
+        float px1 = _ga_t1->getXnormalized();
+        float py1 = _ga_t1->getYnormalized();
+
+        trackball(axis, angle, px1, py1, px0, py0);
+
+        osg::Quat new_rotate;
+        new_rotate.makeRotate(angle, axis);
+
+        _rotation = _rotation * new_rotate;
+
+        return true;
+
+    }
+    else if (buttonMask == GUIEventAdapter::MIDDLE_MOUSE_BUTTON ||
+             buttonMask == (GUIEventAdapter::LEFT_MOUSE_BUTTON |
+                            GUIEventAdapter::RIGHT_MOUSE_BUTTON))
+    {
+        // pan model
+        float scale = -0.3f * _distance;
+
+        osg::Matrix rotation_matrix;
+        rotation_matrix.makeRotate(_rotation);
+
+        osg::Vec3 dv(dx * scale, dy * scale, 0.0f);
+
+        _center += dv * rotation_matrix;
+
+        return true;
+
+    }
+    else if (buttonMask == GUIEventAdapter::RIGHT_MOUSE_BUTTON)
+    {
+        // zoom model
+        float scale = 1.0f + dy;
+        if (_distance * scale > _minZoomRange)
+        {
+
+            _distance *= scale;
+
+        }
+        return true;
+    }
+
+    return false;
+}

src/ui/map3D/GCManipulator.h

+/*=====================================================================
+
+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 GCManipulator.
+ *
+ *   @author Lionel Heng <hengli@student.ethz.ch>
+ *
+ */
+
+#ifndef GCMANIPULATOR_H
+#define GCMANIPULATOR_H
+
+#include <osgGA/TrackballManipulator>
+
+class GCManipulator : public osgGA::TrackballManipulator
+{
+public:
+    GCManipulator();
+
+    void setMinZoomRange(double minZoomRange);
+
+    /**
+     * @brief Handle events.
+     * @return True if event is handled; false otherwise.
+     */
+    virtual bool handle(const osgGA::GUIEventAdapter& ea,
+                        osgGA::GUIActionAdapter& us);
+
+protected:
+    bool calcMovement();
+
+    double _moveSensitivity;
+    double _minZoomRange;
+};
+
+#endif // GCMANIPULATOR_H

src/ui/map3D/Pixhawk3DWidget.cc

@@ -51,8 +51,8 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
      , displayWaypoints(true)
      , followCamera(true)
 {
+    setCameraParams(2.0f, 30.0f, 0.01f, 10000.0f);
     init(15.0f);
-    setCameraParams(0.5f, 30.0f, 0.01f, 10000.0f);
 
     // generate Pixhawk Cheetah model
     egocentricMap->addChild(PixhawkCheetahGeode::instance());
@@ -177,7 +177,7 @@ Pixhawk3DWidget::showTrail(int32_t state)
     {
         if (!displayTrail)
         {
-            trailVertices->clear();
+            trail.clear();
         }
 
         displayTrail = true;
@@ -255,7 +255,7 @@ Pixhawk3DWidget::display(void)
 
     updateHUD(robotX, robotY, robotZ, robotRoll, robotPitch, robotYaw);
     updateTrail(robotX, robotY, robotZ);
-    updateTarget(robotX, robotY, robotZ);
+    updateTarget();
     updateWaypoints();
 
     // set node visibility
@@ -280,38 +280,64 @@ osg::ref_ptr<osg::Geode>
 Pixhawk3DWidget::createGrid(void)
 {
     osg::ref_ptr<osg::Geode> geode(new osg::Geode());
-    osg::ref_ptr<osg::Geometry> geometry(new osg::Geometry());
-    geode->addDrawable(geometry.get());
+    osg::ref_ptr<osg::Geometry> fineGeometry(new osg::Geometry());
+    osg::ref_ptr<osg::Geometry> coarseGeometry(new osg::Geometry());
+    geode->addDrawable(fineGeometry);
+    geode->addDrawable(coarseGeometry);
 
     float radius = 10.0f;
     float resolution = 0.25f;
 
-    osg::ref_ptr<osg::Vec3Array> coords(new osg::Vec3Array);
+    osg::ref_ptr<osg::Vec3Array> fineCoords(new osg::Vec3Array);
+    osg::ref_ptr<osg::Vec3Array> coarseCoords(new osg::Vec3Array);
 
     // draw a 20m x 20m grid with 0.25m resolution
     for (float i = -radius; i <= radius; i += resolution)
     {
-        coords->push_back(osg::Vec3(i, -radius, 0.0f));
-        coords->push_back(osg::Vec3(i, radius, 0.0f));
-        coords->push_back(osg::Vec3(-radius, i, 0.0f));
-        coords->push_back(osg::Vec3(radius, i, 0.0f));
+        if (fabsf(i - roundf(i)) < 0.01f)
+        {
+            coarseCoords->push_back(osg::Vec3(i, -radius, 0.0f));
+            coarseCoords->push_back(osg::Vec3(i, radius, 0.0f));
+            coarseCoords->push_back(osg::Vec3(-radius, i, 0.0f));
+            coarseCoords->push_back(osg::Vec3(radius, i, 0.0f));
+        }
+        else
+        {
+            fineCoords->push_back(osg::Vec3(i, -radius, 0.0f));
+            fineCoords->push_back(osg::Vec3(i, radius, 0.0f));
+            fineCoords->push_back(osg::Vec3(-radius, i, 0.0f));
+            fineCoords->push_back(osg::Vec3(radius, i, 0.0f));
+        }
     }
 
-    geometry->setVertexArray(coords);
+    fineGeometry->setVertexArray(fineCoords);
+    coarseGeometry->setVertexArray(coarseCoords);
 
     osg::ref_ptr<osg::Vec4Array> color(new osg::Vec4Array);
     color->push_back(osg::Vec4(0.5f, 0.5f, 0.5f, 1.0f));
-    geometry->setColorArray(color);
-    geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
-
-    geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, coords->size()));
-
-    osg::ref_ptr<osg::StateSet> stateset(new osg::StateSet);
-    osg::ref_ptr<osg::LineWidth> linewidth(new osg::LineWidth());
-    linewidth->setWidth(0.25f);
-    stateset->setAttributeAndModes(linewidth, osg::StateAttribute::ON);
-    stateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
-    geometry->setStateSet(stateset);
+    fineGeometry->setColorArray(color);
+    coarseGeometry->setColorArray(color);
+    fineGeometry->setColorBinding(osg::Geometry::BIND_OVERALL);
+    coarseGeometry->setColorBinding(osg::Geometry::BIND_OVERALL);
+
+    fineGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES,
+                                                      0, fineCoords->size()));
+    coarseGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0,
+                                                        coarseCoords->size()));
+
+    osg::ref_ptr<osg::StateSet> fineStateset(new osg::StateSet);
+    osg::ref_ptr<osg::LineWidth> fineLinewidth(new osg::LineWidth());
+    fineLinewidth->setWidth(0.25f);
+    fineStateset->setAttributeAndModes(fineLinewidth, osg::StateAttribute::ON);
+    fineStateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
+    fineGeometry->setStateSet(fineStateset);
+
+    osg::ref_ptr<osg::StateSet> coarseStateset(new osg::StateSet);
+    osg::ref_ptr<osg::LineWidth> coarseLinewidth(new osg::LineWidth());
+    coarseLinewidth->setWidth(2.0f);
+    coarseStateset->setAttributeAndModes(coarseLinewidth, osg::StateAttribute::ON);
+    coarseStateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
+    coarseGeometry->setStateSet(coarseStateset);
 
     return geode;
 }
@@ -508,7 +534,7 @@ Pixhawk3DWidget::updateTrail(float robotX, float robotY, float robotZ)
 }
 
 void
-Pixhawk3DWidget::updateTarget(float robotX, float robotY, float robotZ)
+Pixhawk3DWidget::updateTarget(void)
 {
     static double radius = 0.2;
     static bool expand = true;

src/ui/map3D/Pixhawk3DWidget.h

@@ -82,7 +82,7 @@ private:
     void updateHUD(float robotX, float robotY, float robotZ,
                    float robotRoll, float robotPitch, float robotYaw);
     void updateTrail(float robotX, float robotY, float robotZ);
-    void updateTarget(float robotX, float robotY, float robotZ);
+    void updateTarget(void);
     void updateWaypoints(void);
 
     void markTarget(void);

src/ui/map3D/Q3DWidget.cc

@@ -51,7 +51,7 @@ Q3DWidget::Q3DWidget(QWidget* parent)
     , hudProjectionMatrix(new osg::Projection)
 {
     // set initial camera parameters
-    cameraParams.minZoomRange = 0.5f;
+    cameraParams.minZoomRange = 2.0f;
     cameraParams.cameraFov = 30.0f;
     cameraParams.minClipRange = 1.0f;
     cameraParams.maxClipRange = 10000.0f;
@@ -94,8 +94,10 @@ Q3DWidget::init(float fps)
     egocentricMap->addChild(createRobot());
 
     // set up camera control
-    cameraManipulator = new osgGA::TrackballManipulator;
+    cameraManipulator = new GCManipulator;
     setCameraManipulator(cameraManipulator);
+    cameraManipulator->setMinZoomRange(cameraParams.minZoomRange);
+    cameraManipulator->setDistance(cameraParams.minZoomRange * 2.0);
 
     connect(&timer, SIGNAL(timeout()), this, SLOT(redraw()));
     timer.start(static_cast<int>(floorf(1000.0f / fps)));
@@ -282,6 +284,7 @@ Q3DWidget::keyPressEvent(QKeyEvent* event)
     {
         return;
     }
+
     if (event->text().isEmpty())
     {
         osgGW->getEventQueue()->keyPress(convertKey(event->key()));

src/ui/map3D/Q3DWidget.h

@@ -39,6 +39,8 @@ This file is part of the QGROUNDCONTROL project
 #include <osgGA/TrackballManipulator>
 #include <osgViewer/Viewer>
 
+#include "GCManipulator.h"
+
 /**
  * @brief Definition of the class Q3DWidget.
  * The Q3DWidget widget uses the OpenSceneGraph framework to render
@@ -243,7 +245,7 @@ protected:
 
     osg::ref_ptr<osgViewer::GraphicsWindowEmbedded> osgGW; /**< A class which manages OSG graphics windows and events. */
 
-    osg::ref_ptr<osgGA::TrackballManipulator> cameraManipulator; /**< Camera manipulator. */
+    osg::ref_ptr<GCManipulator> cameraManipulator; /**< Camera manipulator. */
 
     QTimer timer; /**< Timer which draws graphics based on specified fps. */