Merge branch 'fixFLSCameraController' into 'master'

ENH: Add translation offset due to angulation

See merge request !177

Base/SceneElements/Controllers/imstkFLSCameraController.cpp

@@ -40,24 +40,62 @@ FLSCameraController::getCameraHeadAngleOffset() const
 }
 
 void
-FLSCameraController::setCameraAngulationOffset(const Quatd& r)
+FLSCameraController::setCameraAngulationOffset(const double angle)
 {
-    m_cameraAngulationOffset = r;
+    m_cameraAngulationOffset = angle;
+    m_cameraAngulationRotOffset = Quatd(Eigen::AngleAxisd(angle*PI / 180., Vec3d(0., 1., 0.)));
 }
 
-const imstk::Quatd&
+double
 FLSCameraController::getCameraAngulationOffset() const
 {
     return m_cameraAngulationOffset;
 }
 
+void
+FLSCameraController::setCameraAngulationTranslationOffset(const double t)
+{
+    m_angulationTranslationOffset = t;
+}
+
+double
+FLSCameraController::getCameraAngulationTranslationOffset() const
+{
+    return m_angulationTranslationOffset;
+}
+
 void
 FLSCameraController::runModule()
 {
     auto roff = Quatd(Eigen::AngleAxisd(m_cameraHeadAngleOffset*PI / 180., Vec3d(0., 0., 1.)));
-    roff *= m_cameraAngulationOffset;
-    this->setRotationOffset(roff);
+    roff *= m_cameraAngulationRotOffset;
+    this->setCameraRotationOffset(roff);
+
+    if (!m_trackingDataUptoDate)
+    {
+        if (!updateTrackingData())
+        {
+            LOG(WARNING) << "CameraController::runModule warning: could not update tracking info.";
+            return;
+        }
+    }
+
+    Vec3d p = getPosition();
+    Quatd r = getRotation();
+
+    m_cameraTranslationOffset = r*Vec3d(m_angulationTranslationOffset*cos(m_cameraAngulationOffset*PI / 180.),
+                                        0.,
+                                        m_angulationTranslationOffset*sin(m_cameraAngulationOffset*PI / 180.));
+
+    // Apply Offsets over the device pose
+    p += m_cameraTranslationOffset;      // Offset the device position
+    r *= m_cameraRotationalOffset;       // Apply camera head rotation offset
+
+    // Set camera pose
+    m_camera.setPosition(p);
+    m_camera.setFocalPoint(r*FORWARD_VECTOR + p);
+    m_camera.setViewUp(r*UP_VECTOR);
 
-    CameraController::runModule();
+    m_trackingDataUptoDate = false;
 }
 } // imstk

Base/SceneElements/Controllers/imstkFLSCameraController.h

@@ -56,8 +56,14 @@ public:
     ///
     /// \brief Get/Set the rotation offset due to angulation
     ///
-    void setCameraAngulationOffset(const Quatd& r);
-    const Quatd& getCameraAngulationOffset() const;
+    void setCameraAngulationOffset(const double angle);
+    double getCameraAngulationOffset() const;
+
+    ///
+    /// \brief Get/Set the translation offset due to angulation
+    ///
+    void setCameraAngulationTranslationOffset(const double t);
+    double getCameraAngulationTranslationOffset() const;
 
 protected:
 
@@ -67,7 +73,9 @@ protected:
     void runModule() override;
 
     double m_cameraHeadAngleOffset = 0;                   ///< camera head angle offset (in deg)
-    Quatd m_cameraAngulationOffset = Quatd::Identity();   ///< Rotation offset for the camera via telescope angulation
+    double m_cameraAngulationOffset = 0.;                 ///< Rotation offset for the camera via telescope angulation
+    Quatd m_cameraAngulationRotOffset = Quatd::Identity();///< Rotation offset for the camera via telescope angulation
+    double m_angulationTranslationOffset = 0.1;
 };
 } // imstk