Improved implementation of follow camera feature.

src/ui/map3D/GCManipulator.cc

@@ -33,16 +33,23 @@ This file is part of the QGROUNDCONTROL project
 
 GCManipulator::GCManipulator()
 {
-    _moveSensitivity = 0.05;
-    _minZoomRange = 2.0;
+    _moveSensitivity = 0.05f;
+    _zoomSensitivity = 1.0f;
+    _minZoomRange = 2.0f;
 }
 
 void
-GCManipulator::setMinZoomRange(double minZoomRange)
+GCManipulator::setMinZoomRange(float minZoomRange)
 {
     _minZoomRange = minZoomRange;
 }
 
+void
+GCManipulator::move(float dx, float dy, float dz)
+{
+    _center += osg::Vec3(dx, dy, dz);
+}
+
 bool
 GCManipulator::handle(const osgGA::GUIEventAdapter& ea,
                       osgGA::GUIActionAdapter& us)
@@ -123,11 +130,11 @@ GCManipulator::handle(const osgGA::GUIEventAdapter& ea,
 
             if (ea.getScrollingMotion() == GUIEventAdapter::SCROLL_UP)
             {
-                scale -= 0.1f;
+                scale -= _zoomSensitivity * 0.1f;
             }
             else
             {
-                scale += 0.1f;
+                scale += _zoomSensitivity * 0.1f;
             }
             if (_distance * scale > _minZoomRange)
             {
@@ -271,7 +278,7 @@ GCManipulator::calcMovement()
                             GUIEventAdapter::RIGHT_MOUSE_BUTTON))
     {
         // pan model
-        float scale = -0.3f * _distance;
+        float scale = -_moveSensitivity * _distance;
 
         osg::Matrix rotation_matrix;
         rotation_matrix.makeRotate(_rotation);
@@ -286,7 +293,7 @@ GCManipulator::calcMovement()
     else if (buttonMask == GUIEventAdapter::RIGHT_MOUSE_BUTTON)
     {
         // zoom model
-        float scale = 1.0f + dy;
+        float scale = 1.0f + dy * _zoomSensitivity;
         if (_distance * scale > _minZoomRange)
         {
 

src/ui/map3D/GCManipulator.h

@@ -39,7 +39,9 @@ class GCManipulator : public osgGA::TrackballManipulator
 public:
     GCManipulator();
 
-    void setMinZoomRange(double minZoomRange);
+    void setMinZoomRange(float minZoomRange);
+
+    virtual void move(float dx, float dy, float dz);
 
     /**
      * @brief Handle events.
@@ -51,8 +53,9 @@ public:
 protected:
     bool calcMovement();
 
-    double _moveSensitivity;
-    double _minZoomRange;
+    float _moveSensitivity;
+    float _zoomSensitivity;
+    float _minZoomRange;
 };
 
 #endif // GCMANIPULATOR_H

src/ui/map3D/Pixhawk3DWidget.cc

@@ -50,6 +50,9 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
      , displayTarget(false)
      , displayWaypoints(true)
      , followCamera(true)
+     , lastRobotX(0.0f)
+     , lastRobotY(0.0f)
+     , lastRobotZ(0.0f)
 {
     setCameraParams(2.0f, 30.0f, 0.01f, 10000.0f);
     init(15.0f);
@@ -137,7 +140,7 @@ Pixhawk3DWidget::buildLayout(void)
             this, SLOT(showTrail(int)));
     connect(waypointsCheckBox, SIGNAL(stateChanged(int)),
             this, SLOT(showWaypoints(int)));
-    connect(recenterButton, SIGNAL(clicked()), this, SLOT(recenterCamera()));
+    connect(recenterButton, SIGNAL(clicked()), this, SLOT(recenter()));
     connect(followCameraCheckBox, SIGNAL(stateChanged(int)),
             this, SLOT(toggleFollowCamera(int)));
 }
@@ -202,7 +205,7 @@ Pixhawk3DWidget::showWaypoints(int state)
 }
 
 void
-Pixhawk3DWidget::recenterCamera(void)
+Pixhawk3DWidget::recenter(void)
 {
     float robotX = 0.0f, robotY = 0.0f, robotZ = 0.0f;
     if (uas != NULL)
@@ -212,7 +215,7 @@ Pixhawk3DWidget::recenterCamera(void)
         robotZ = uas->getLocalZ();
     }
 
-    recenter(robotY, robotX, -robotZ);
+    recenterCamera(robotY, robotX, -robotZ);
 }
 
 void
@@ -231,21 +234,36 @@ Pixhawk3DWidget::toggleFollowCamera(int32_t state)
 void
 Pixhawk3DWidget::display(void)
 {
-    float robotX = 0.0f, robotY = 0.0f, robotZ = 0.0f;
-    float robotRoll = 0.0f, robotPitch = 0.0f, robotYaw = 0.0f;
-    if (uas != NULL)
+    if (uas == NULL)
     {
-        robotX = uas->getLocalX();
-        robotY = uas->getLocalY();
-        robotZ = uas->getLocalZ();
-        robotRoll = uas->getRoll();
-        robotPitch = uas->getPitch();
-        robotYaw = uas->getYaw();
+        return;
+    }
+
+    float robotX = uas->getLocalX();
+    float robotY = uas->getLocalY();
+    float robotZ = uas->getLocalZ();
+    float robotRoll = uas->getRoll();
+    float robotPitch = uas->getPitch();
+    float robotYaw = uas->getYaw();
+
+    if (lastRobotX == 0.0f && lastRobotY == 0.0f && lastRobotZ == 0.0f)
+    {
+        lastRobotX = robotX;
+        lastRobotY = robotY;
+        lastRobotZ = robotZ;
+
+        recenterCamera(robotY, robotX, -robotZ);
+
+        return;
     }
 
     if (followCamera)
     {
-        recenter(robotY, robotX, -robotZ);
+        float dx = robotY - lastRobotY;
+        float dy = robotX - lastRobotX;
+        float dz = lastRobotZ - robotZ;
+
+        moveCamera(dx, dy, dz);
     }
 
     robotPosition->setPosition(osg::Vec3(robotY, robotX, -robotZ));
@@ -263,6 +281,10 @@ Pixhawk3DWidget::display(void)
     rollingMap->setChildValue(trailNode, displayTrail);
     rollingMap->setChildValue(targetNode, displayTarget);
     rollingMap->setChildValue(waypointsNode, displayWaypoints);
+
+    lastRobotX = robotX;
+    lastRobotY = robotY;
+    lastRobotZ = robotZ;
 }
 
 void

src/ui/map3D/Q3DWidget.cc

@@ -181,7 +181,13 @@ Q3DWidget::setCameraParams(float minZoomRange, float cameraFov,
 }
 
 void
-Q3DWidget::recenter(float x, float y, float z)
+Q3DWidget::moveCamera(float dx, float dy, float dz)
+{
+    cameraManipulator->move(dx, dy, dz);
+}
+
+void
+Q3DWidget::recenterCamera(float x, float y, float z)
 {
     cameraManipulator->setCenter(osg::Vec3d(x, y, z));
 }

src/ui/map3D/Q3DWidget.h

@@ -81,10 +81,18 @@ public:
     void setCameraParams(float minZoomRange, float cameraFov,
                          float minClipRange, float maxClipRange);
 
+    /**
+     * @brief Moves the camera by [dx,dy,dz].
+     * @param dx Translation along the x-axis in meters.
+     * @param dy Translation along the y-axis in meters.
+     * @param dz Translation along the z-axis in meters.
+     */
+    void moveCamera(float dx, float dy, float dz);
+
     /**
      * @brief Recenters the camera at (x,y,z).
      */
-    void recenter(float x, float y, float z);
+    void recenterCamera(float x, float y, float z);
 
     /**
      * @brief Sets up 3D display mode.