diff --git a/Base/SceneElements/Controllers/imstkCameraController.cpp b/Base/SceneElements/Controllers/imstkCameraController.cpp index e6e68f2fcccdad6d7685555e83c9e85ff2824d5c..f68c00a6f2695caf8e24967fe352b0fb47829800 100644 --- a/Base/SceneElements/Controllers/imstkCameraController.cpp +++ b/Base/SceneElements/Controllers/imstkCameraController.cpp @@ -30,20 +30,6 @@ namespace imstk void CameraController::initModule() { - auto pos = m_camera.getPosition(); - auto viewUp = m_camera.getViewUp(); - auto focus = m_camera.getFocalPoint(); - - m_translationOffset = pos; - - auto viewNormal = (pos - focus).normalized(); - auto viewSide = viewUp.cross(viewNormal).normalized(); - viewUp = viewNormal.cross(viewSide); - Mat3d rot; - rot.col(0) = viewSide; - rot.col(1) = viewUp; - rot.col(2) = viewNormal; - m_rotationOffset = Quatd(rot); } void @@ -58,8 +44,12 @@ CameraController::runModule() } } - const Vec3d p = getPosition(); - const Quatd r = getRotation(); + Vec3d p = getPosition(); + Quatd r = getRotation(); + + // Apply Offsets over the device pose + p = p + m_cameraTranslationOffset; // Offset the device position + r *= m_cameraRotationalOffset; // Apply camera head rotation offset // Set camera info m_camera.setPosition(p); @@ -70,7 +60,45 @@ CameraController::runModule() } void -CameraController::cleanUpModule() +CameraController::setOffsetUsingCurrentCameraPose() +{ + auto pos = m_camera.getPosition(); + auto viewUp = m_camera.getViewUp(); + auto focus = m_camera.getFocalPoint(); + + m_translationOffset = pos; + + auto viewNormal = (pos - focus).normalized(); + auto viewSide = viewUp.cross(viewNormal).normalized(); + viewUp = viewNormal.cross(viewSide); + Mat3d rot; + rot.col(0) = viewSide; + rot.col(1) = viewUp; + rot.col(2) = viewNormal; + m_rotationOffset = Quatd(rot); +} + +void +CameraController::setCameraRotationOffset(const Quatd& r) +{ + m_cameraRotationalOffset = r; +} + +void +CameraController::setCameraTranslationOffset(const Vec3d& t) +{ + m_cameraTranslationOffset = t; +} + +const Vec3d& +CameraController::getCameraTranslationOffset() const +{ + return m_cameraTranslationOffset; +} + +const Quatd& +CameraController::getCameraRotationOffset() const { + return m_cameraRotationalOffset; } } // imstk diff --git a/Base/SceneElements/Controllers/imstkCameraController.h b/Base/SceneElements/Controllers/imstkCameraController.h index caa0a6423c41e10a0e837549658c9db8f48c222d..6968734b0a4594f7d71ec72af2526edf1a62c4d1 100644 --- a/Base/SceneElements/Controllers/imstkCameraController.h +++ b/Base/SceneElements/Controllers/imstkCameraController.h @@ -53,23 +53,44 @@ public: /// ~CameraController() = default; + /// + /// \brief Set the offsets based on the current camera pose + /// + void setOffsetUsingCurrentCameraPose(); + + /// + /// \brief Get/Set translation offset of the camera + /// + const Vec3d& getCameraTranslationOffset() const; + void setCameraTranslationOffset(const Vec3d& t); + + /// + /// \brief Get/Set rotation offset of the camera + /// + const Quatd& getCameraRotationOffset() const; + void setCameraRotationOffset(const Quatd& r); + + protected: /// /// \brief /// - void initModule() override; + virtual void initModule() override; /// /// \brief /// - void runModule() override; + virtual void runModule() override; /// /// \brief /// - void cleanUpModule() override; + void cleanUpModule() override {}; Camera& m_camera; ///< Camera controlled by the external device + + Vec3d m_cameraTranslationOffset = WORLD_ORIGIN; ///< Translation offset for the camera over tracking data + Quatd m_cameraRotationalOffset = Quatd::Identity(); ///< camera head angle offset (in deg) }; } // imstk diff --git a/Base/SceneElements/Controllers/imstkSceneObjectController.h b/Base/SceneElements/Controllers/imstkSceneObjectController.h index a4bf690194661adbe200ddce65865a7458fddf25..49beb3f8ec3263860b60c8d999590921c108ddd1 100644 --- a/Base/SceneElements/Controllers/imstkSceneObjectController.h +++ b/Base/SceneElements/Controllers/imstkSceneObjectController.h @@ -37,7 +37,7 @@ namespace imstk /// class SceneObjectController : public SceneObjectControllerBase { - using ControllerCallbackFunction = std::function<void(SceneObjectController* hdapiClient)>; +using ControllerCallbackFunction = std::function<void(SceneObjectController* hdapiClient)>; public: /// /// \brief Constructor