ENH: Init controllers offsets on simulation start

1) for camera controller, setup offsets in initModule
based on the camera transform

2) for object controller, setup offset in sceneManager
initModule based on the object colliding geometry transform

This API allows the user to only worry about the geometries
and camera transforms, and let the controllers auto-init based
on that information when the simulation starts only.

Base/Scene/Camera/imstkCamera.cpp

@@ -105,7 +105,7 @@ Camera::getController() const
     return m_cameraController;
 }
 
-std::shared_ptr<CameraController>
+void
 Camera::setupController(std::shared_ptr<DeviceClient> deviceClient, double scaling)
 {
     if(m_cameraController == nullptr)
@@ -114,17 +114,5 @@ Camera::setupController(std::shared_ptr<DeviceClient> deviceClient, double scali
     }
     m_cameraController->setDeviceClient(deviceClient);
     m_cameraController->setTranslationScaling(scaling);
-    m_cameraController->setTranslationOffset(m_position);
-
-    auto viewNormal = (m_position - m_focalPoint).normalized();
-    auto viewSide = m_viewUp.cross(viewNormal).normalized();
-    m_viewUp = viewNormal.cross(viewSide);
-    Mat3d rot;
-    rot.col(0) = viewSide;
-    rot.col(1) = m_viewUp;
-    rot.col(2) = viewNormal;
-    m_cameraController->setRotationOffset(Quatd(rot));
-
-    return m_cameraController;
 }
 }

Base/Scene/Camera/imstkCamera.h

@@ -61,7 +61,7 @@ public:
     void setViewAngle(const double& angle);
 
     std::shared_ptr<CameraController> getController() const;
-    std::shared_ptr<CameraController> setupController(std::shared_ptr<DeviceClient> deviceClient, double scaling = 1.0);
+    void setupController(std::shared_ptr<DeviceClient> deviceClient, double scaling = 1.0);
 
 protected:
 

Base/Scene/Controllers/imstkCameraController.cpp

@@ -30,7 +30,20 @@ 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

Base/Scene/Object/imstkVirtualCouplingObject.cpp

@@ -30,6 +30,13 @@
 
 namespace imstk {
 
+void
+VirtualCouplingObject::initOffsets()
+{
+    m_translationOffset = m_collidingGeometry->getPosition();
+    m_rotationOffset = m_collidingGeometry->getOrientation();
+}
+
 void
 VirtualCouplingObject::updateFromDevice()
 {

Base/Scene/Object/imstkVirtualCouplingObject.h

@@ -45,6 +45,14 @@ public:
 
     ~VirtualCouplingObject() = default;
 
+    ///
+    /// \brief Initialise offset based on object geometry
+    ///
+    void initOffsets();
+
+    ///
+    /// \brief Update geometries transformations
+    ///
     void updateFromDevice();
 
 };

Base/SimulationManager/imstkSceneManager.cpp

@@ -42,6 +42,15 @@ SceneManager::initModule()
     {
         this->startModuleInNewThread(camController);
     }
+
+    // Init virtual coupling objects offsets
+    for (auto obj : m_scene->getSceneObjects())
+    {
+        if (auto virtualCoupling = std::dynamic_pointer_cast<VirtualCouplingObject>(obj))
+        {
+            virtualCoupling->initOffsets();
+        }
+    }
 }
 
 void

Examples/Sandbox/main.cpp

@@ -91,12 +91,11 @@ void testObjectController()
 
     // Sphere
     auto sphereGeom = std::make_shared<imstk::Sphere>();
-    sphereGeom->setPosition(imstk::UP_VECTOR); //does not matter, need to set offset on object if controlled
+    sphereGeom->setPosition(imstk::UP_VECTOR);
     sphereGeom->scale(0.2);
     auto sphereObj = std::make_shared<imstk::VirtualCouplingObject>("VirtualSphere", client, 20);
     sphereObj->setVisualGeometry(sphereGeom);
     sphereObj->setCollidingGeometry(sphereGeom);
-    sphereObj->setTranslationOffset(imstk::UP_VECTOR); // this will work though
     scene->addSceneObject(sphereObj);
 
     // Update Camera position
@@ -135,9 +134,9 @@ void testCameraController()
     cam->setPosition(imstk::Vec3d(8,-8,8));
 
     // Set camera controller
-    auto controller = cam->setupController(client, 100);
-    //LOG(INFO) << controller->getTranslationOffset(); // should be the same than initial cam position
-    //controller->setInversionFlags( (imstk::CameraController::InvertFlag::transX | imstk::CameraController::InvertFlag::transY) );
+    cam->setupController(client, 100);
+    //LOG(INFO) << cam->getController()->getTranslationOffset(); // should be the same than initial cam position
+    //cam->getController()->setInversionFlags( (imstk::CameraController::InvertFlag::transX | imstk::CameraController::InvertFlag::transY) );
 
     // Run
     sdk->setCurrentScene("SceneTestDevice");