diff --git a/Base/Devices/imstkHDAPIDeviceClient.cpp b/Base/Devices/imstkHDAPIDeviceClient.cpp
index b62b437afe1a2d0c161a1919d0a7fd03287a88ac..4ebddbfb75ca79fc3981573918d4d74315a132cf 100644
--- a/Base/Devices/imstkHDAPIDeviceClient.cpp
+++ b/Base/Devices/imstkHDAPIDeviceClient.cpp
@@ -34,6 +34,8 @@ namespace imstk
void
HDAPIDeviceClient::init()
{
+ m_buttons = std::map < size_t, bool > { { 0, false }, { 1, false }, { 2, false }, { 3, false }};
+
// Open Device
m_handle = hdInitDevice(this->getDeviceName().c_str());
diff --git a/Base/Devices/imstkHDAPIDeviceClient.h b/Base/Devices/imstkHDAPIDeviceClient.h
index d0483065fa659966ec9c0f2ff6ea678e15af7bb4..a6c1254df491259fe0307cd4c26702178bc5303a 100644
--- a/Base/Devices/imstkHDAPIDeviceClient.h
+++ b/Base/Devices/imstkHDAPIDeviceClient.h
@@ -32,7 +32,8 @@
#include <memory>
-namespace imstk {
+namespace imstk
+{
struct HD_state
{
@@ -50,25 +51,39 @@ class HDAPIDeviceClient : public DeviceClient
{
public:
- HDAPIDeviceClient(std::string name):
- DeviceClient(name, "localhost")
- {}
-
- virtual ~HDAPIDeviceClient() {}
+ ///
+ /// \brief Constructor/Destructor
+ ///
+ HDAPIDeviceClient(std::string name): DeviceClient(name, "localhost"){}
+ virtual ~HDAPIDeviceClient(){}
protected:
friend class HDAPIDeviceServer;
+
+ ///
+ /// \brief Initialize the phantom omni device
+ ///
void init();
+
+ ///
+ /// \brief Use callback to get tracking data from phantom omni
+ ///
void run();
+
+ ///
+ /// \brief Closes the phantom omni device
+ ///
void cleanUp();
private:
-
+ ///
+ /// \brief Phantom omni device api callback
+ ///
static HDCallbackCode HDCALLBACK hapticCallback(void* pData);
- HHD m_handle; //!< device handle
- HD_state m_state; //!< device reading state
+ HHD m_handle; ///< device handle
+ HD_state m_state; ///< device reading state
};
}
diff --git a/Base/Scene/imstkScene.cpp b/Base/Scene/imstkScene.cpp
index 2983ebf0c503263f3d5dd93cb9673ff231174bb0..73360e8d4ab567f7dbd4a0edd2a4887770666cbc 100644
--- a/Base/Scene/imstkScene.cpp
+++ b/Base/Scene/imstkScene.cpp
@@ -21,6 +21,7 @@
#include "imstkScene.h"
#include "imstkCameraController.h"
+#include "imstkSceneObjectControllerBase.h"
#include <g3log/g3log.hpp>
@@ -48,6 +49,12 @@ Scene::getSceneObjects() const
return v;
}
+const std::vector<std::shared_ptr<SceneObjectControllerBase>>
+Scene::getSceneObjectControllers() const
+{
+ return m_objectControllers;
+}
+
std::shared_ptr<SceneObject>
Scene::getSceneObject(std::string sceneObjectName) const
{
@@ -62,7 +69,7 @@ Scene::getSceneObject(std::string sceneObjectName) const
}
void
-Scene::addSceneObject(std::shared_ptr<SceneObject>newSceneObject)
+Scene::addSceneObject(std::shared_ptr<SceneObject> newSceneObject)
{
std::string newSceneObjectName = newSceneObject->getName();
@@ -184,4 +191,9 @@ void Scene::addNonlinearSolver(std::shared_ptr<SolverBase> solver)
m_solvers.push_back(solver);
}
+void Scene::addObjectController(std::shared_ptr<SceneObjectControllerBase> controller)
+{
+ m_objectControllers.push_back(controller);
+}
+
} // imstk
\ No newline at end of file
diff --git a/Base/Scene/imstkScene.h b/Base/Scene/imstkScene.h
index 4191cd3bd2760b8a3db5ad8ec4a77d4d4edc4fc6..1a753b91fc3e502400edff7d3017be7c1d6a635f 100644
--- a/Base/Scene/imstkScene.h
+++ b/Base/Scene/imstkScene.h
@@ -34,6 +34,8 @@
namespace imstk
{
+class SceneObjectControllerBase;
+
///
/// \class Scene
///
@@ -67,6 +69,11 @@ public:
///
const std::vector<std::shared_ptr<SceneObject>> getSceneObjects() const;
+ ///
+ /// \brief Get the scene object controllers
+ ///
+ const std::vector <std::shared_ptr<SceneObjectControllerBase>> getSceneObjectControllers() const;
+
///
/// \brief Get a scene object of a specific name
///
@@ -124,6 +131,11 @@ public:
///
void addNonlinearSolver(std::shared_ptr<SolverBase> solver);
+ ///
+ /// \brief Add objects controllers
+ ///
+ void addObjectController(std::shared_ptr<SceneObjectControllerBase> controller);
+
protected:
std::string m_name; ///> Name of the scene
@@ -131,7 +143,8 @@ protected:
NamedMap<Light> m_lightsMap;
std::shared_ptr<Camera> m_camera = std::make_shared<Camera>();
std::shared_ptr<CollisionGraph> m_collisionGraph = std::make_shared<CollisionGraph>();
- std::vector<std::shared_ptr<SolverBase>> m_solvers; ///> List of non-linear solvers
+ std::vector<std::shared_ptr<SolverBase>> m_solvers; ///> List of non-linear solvers
+ std::vector<std::shared_ptr<SceneObjectControllerBase>> m_objectControllers; ///> List of controllers
};
} // imstk
diff --git a/Base/SceneElements/Controllers/imstkCameraController.cpp b/Base/SceneElements/Controllers/imstkCameraController.cpp
index 06272f2e50ae9be67935c86fa66343a504a78024..2162cf14c68c6a61bfa7da73d727d74e41fa1116 100644
--- a/Base/SceneElements/Controllers/imstkCameraController.cpp
+++ b/Base/SceneElements/Controllers/imstkCameraController.cpp
@@ -50,15 +50,18 @@ CameraController::initModule()
void
CameraController::runModule()
{
- Vec3d p;
- Quatd r;
-
- if (!this->computeTrackingData(p, r))
+ if (!m_trackingDataUptoDate)
{
- LOG(WARNING) << "CameraController::runModule warning: could not update tracking info.";
- return;
+ if (!updateTrackingData())
+ {
+ LOG(WARNING) << "CameraController::runModule warning: could not update tracking info.";
+ return;
+ }
}
+ Vec3d p = getPosition();
+ Quatd r = getRotation();
+
// Set camera info
m_camera.setPosition(p);
m_camera.setFocalPoint((r*FORWARD_VECTOR)+p);
diff --git a/Base/SceneElements/Controllers/imstkCameraController.h b/Base/SceneElements/Controllers/imstkCameraController.h
index 2e2c94a28b3c6c4e6577a2c2bcfe80797b1c1980..b5958d4d320759e99a2384358c9096753a3f5087 100644
--- a/Base/SceneElements/Controllers/imstkCameraController.h
+++ b/Base/SceneElements/Controllers/imstkCameraController.h
@@ -23,7 +23,7 @@
#define imstkCameraController_h
#include "imstkModule.h"
-#include "imstkTrackingController.h"
+#include "imstkDeviceTracker.h"
#include "imstkCamera.h"
#include <memory>
@@ -36,7 +36,7 @@ namespace imstk
///
/// \brief
///
-class CameraController : public Module, public TrackingController
+class CameraController : public Module, public DeviceTracker
{
public:
///
@@ -46,7 +46,7 @@ public:
std::shared_ptr<DeviceClient> deviceClient) :
Module("Camera controller"),
m_camera(camera),
- TrackingController(deviceClient)
+ DeviceTracker(deviceClient)
{}
///
diff --git a/Base/SceneElements/Controllers/imstkTrackingController.cpp b/Base/SceneElements/Controllers/imstkDeviceTracker.cpp
similarity index 52%
rename from Base/SceneElements/Controllers/imstkTrackingController.cpp
rename to Base/SceneElements/Controllers/imstkDeviceTracker.cpp
index 41422cf96aa6370fabb113c2caee7d2867374f14..ee4ad6100821dc2057d4107b889c73d6c33d4133 100644
--- a/Base/SceneElements/Controllers/imstkTrackingController.cpp
+++ b/Base/SceneElements/Controllers/imstkDeviceTracker.cpp
@@ -19,7 +19,7 @@
=========================================================================*/
-#include "imstkTrackingController.h"
+#include "imstkDeviceTracker.h"
#include <utility>
@@ -29,89 +29,91 @@ namespace imstk
{
bool
-TrackingController::computeTrackingData(Vec3d& p, Quatd& r)
+DeviceTracker::updateTrackingData()
{
if (m_deviceClient == nullptr)
{
- LOG(WARNING) << "TrackingController::getTrackingData warning: no controlling device set.";
+ LOG(WARNING) << "DeviceTracker::getTrackingData warning: no controlling device set.";
return false;
}
// Retrieve device info
- p = m_deviceClient->getPosition();
- r = m_deviceClient->getOrientation();
+ m_currentPos = m_deviceClient->getPosition();
+ m_currentRot = m_deviceClient->getOrientation();
// Apply inverse if needed
- if(m_invertFlags & InvertFlag::transX) p[0] = -p[0];
- if(m_invertFlags & InvertFlag::transY) p[1] = -p[1];
- if(m_invertFlags & InvertFlag::transZ) p[2] = -p[2];
- if(m_invertFlags & InvertFlag::rotX) r.x() = -r.x();
- if(m_invertFlags & InvertFlag::rotY) r.y() = -r.y();
- if(m_invertFlags & InvertFlag::rotZ) r.z() = -r.z();
+ if (m_invertFlags & InvertFlag::transX) m_currentPos[0] = -m_currentPos[0];
+ if (m_invertFlags & InvertFlag::transY) m_currentPos[1] = -m_currentPos[1];
+ if (m_invertFlags & InvertFlag::transZ) m_currentPos[2] = -m_currentPos[2];
+ if (m_invertFlags & InvertFlag::rotX) m_currentRot.x() = -m_currentRot.x();
+ if (m_invertFlags & InvertFlag::rotY) m_currentRot.y() = -m_currentRot.y();
+ if (m_invertFlags & InvertFlag::rotZ) m_currentRot.z() = -m_currentRot.z();
// Apply Offsets
- p = m_rotationOffset * p * m_scaling + m_translationOffset;
- r *= m_rotationOffset;
+ m_currentPos = m_rotationOffset * m_currentPos * m_scaling + m_translationOffset;
+ m_currentRot *= m_rotationOffset;
+
+ m_trackingDataUptoDate = true;
return true;
}
std::shared_ptr<DeviceClient>
-TrackingController::getDeviceClient() const
+DeviceTracker::getDeviceClient() const
{
return m_deviceClient;
}
void
-TrackingController::setDeviceClient(std::shared_ptr<DeviceClient> deviceClient)
+DeviceTracker::setDeviceClient(std::shared_ptr<DeviceClient> deviceClient)
{
m_deviceClient = deviceClient;
}
double
-TrackingController::getTranslationScaling() const
+DeviceTracker::getTranslationScaling() const
{
return m_scaling;
}
void
-TrackingController::setTranslationScaling(double scaling)
+DeviceTracker::setTranslationScaling(double scaling)
{
m_scaling = scaling;
}
const Vec3d&
-TrackingController::getTranslationOffset() const
+DeviceTracker::getTranslationOffset() const
{
return m_translationOffset;
}
void
-TrackingController::setTranslationOffset(const Vec3d& t)
+DeviceTracker::setTranslationOffset(const Vec3d& t)
{
m_translationOffset = t;
}
const Quatd&
-TrackingController::getRotationOffset()
+DeviceTracker::getRotationOffset()
{
return m_rotationOffset;
}
void
-TrackingController::setRotationOffset(const Quatd& r)
+DeviceTracker::setRotationOffset(const Quatd& r)
{
m_rotationOffset = r;
}
unsigned char
-TrackingController::getInversionFlags()
+DeviceTracker::getInversionFlags()
{
return m_invertFlags;
}
void
-TrackingController::setInversionFlags(unsigned char f)
+DeviceTracker::setInversionFlags(unsigned char f)
{
m_invertFlags = f;
}
diff --git a/Base/SceneElements/Controllers/imstkTrackingController.h b/Base/SceneElements/Controllers/imstkDeviceTracker.h
similarity index 69%
rename from Base/SceneElements/Controllers/imstkTrackingController.h
rename to Base/SceneElements/Controllers/imstkDeviceTracker.h
index a68221a314f080cd61b518e0148d0e2e6bc3fa73..c6734321ec3f672e4e0b122aa28e93b32409255f 100644
--- a/Base/SceneElements/Controllers/imstkTrackingController.h
+++ b/Base/SceneElements/Controllers/imstkDeviceTracker.h
@@ -19,8 +19,8 @@
=========================================================================*/
-#ifndef imstkTrackingController_h
-#define imstkTrackingController_h
+#ifndef imstkDeviceTracker_h
+#define imstkDeviceTracker_h
#include <memory>
@@ -32,11 +32,11 @@ namespace imstk
{
///
-/// \class TrackingController
+/// \class DeviceTracker
///
/// \brief This class reports external device's position and orientation with a given offset
///
-class TrackingController
+class DeviceTracker
{
public:
@@ -45,20 +45,26 @@ public:
transX = 0x01,
transY = 0x02,
transZ = 0x04,
- rotX = 0x08,
- rotY = 0x10,
- rotZ = 0x20
+ rotX = 0x08,
+ rotY = 0x10,
+ rotZ = 0x20
};
+ ///
+ /// \brief Constructor
+ ///
+ DeviceTracker(std::shared_ptr<DeviceClient> deviceClient) :
+ m_deviceClient(deviceClient) {}
+
///
/// \brief Destructor
///
- ~TrackingController() = default;
+ ~DeviceTracker() = default;
///
/// \brief Compute the world position and orientation
///
- bool computeTrackingData(Vec3d& p, Quatd& r);
+ bool updateTrackingData();
///
/// \brief Get/Set the device client
@@ -90,21 +96,40 @@ public:
unsigned char getInversionFlags();
void setInversionFlags(unsigned char f);
-protected:
///
- /// \brief Constructor
+ /// \brief Sets the tracking data to be out of date or up to date
+ ///
+ void setTrackerToOutOfDate() { m_trackingDataUptoDate = false; }
+ void setTrackerToUpToDate() { m_trackingDataUptoDate = true; }
+
+ ///
+ /// \brief Returns true if the tracking data is already updated in current frame. Else, false.
///
- TrackingController(std::shared_ptr<DeviceClient> deviceClient) :
- m_deviceClient(deviceClient)
- {}
+ bool isTrackerUpToDate() const { return m_trackingDataUptoDate; }
+
+ ///
+ /// \brief Get the latest position
+ ///
+ const Vec3d& getPosition(){ return m_currentPos; };
+
+ ///
+ /// \brief Get the latest rotation
+ ///
+ const Quatd& getRotation() { return m_currentRot; };
+
+protected:
std::shared_ptr<DeviceClient> m_deviceClient; ///< Reports device tracking information
double m_scaling; ///< Scaling factor for physical to virtual translations
Vec3d m_translationOffset = WORLD_ORIGIN; ///< Translation concatenated to the device translation
Quatd m_rotationOffset = Quatd::Identity(); ///< Rotation concatenated to the device rotation
- unsigned char m_invertFlags = 0x00; ///< Invert flags to be masked with TrackingController::InvertFlag
+ unsigned char m_invertFlags = 0x00; ///< Invert flags to be masked with DeviceTracker::InvertFlag
+
+ Vec3d m_currentPos;
+ Quatd m_currentRot;
+ bool m_trackingDataUptoDate = false;
};
} // imstk
-#endif // ifndef imstkTrackingController_h
+#endif // ifndef imstkDeviceTracker_h
diff --git a/Base/SceneElements/Controllers/imstkLaparoscopicToolController.cpp b/Base/SceneElements/Controllers/imstkLaparoscopicToolController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..53d6e6d737ba4d615f154f9a7dcd45b9e71f49ea
--- /dev/null
+++ b/Base/SceneElements/Controllers/imstkLaparoscopicToolController.cpp
@@ -0,0 +1,111 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0.txt
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+
+ =========================================================================*/
+
+#include "imstkLaparoscopicToolController.h"
+
+#include "imstkCollidingObject.h"
+#include "imstkGeometry.h"
+
+#include <utility>
+
+#include <g3log/g3log.hpp>
+
+namespace imstk
+{
+
+void
+LaparoscopicToolController::initOffsets()
+{
+ m_trackingController->setTranslationOffset(m_shaft->getMasterGeometry()->getPosition());
+ m_trackingController->setRotationOffset(m_shaft->getMasterGeometry()->getOrientation());
+
+ m_trackingController->getDeviceClient()->setButtonsEnabled(true);
+}
+
+void
+LaparoscopicToolController::updateControlledObjects()
+{
+ if (!m_trackingController->isTrackerUpToDate())
+ {
+ if (!m_trackingController->updateTrackingData())
+ {
+ LOG(WARNING) << "LaparoscopicToolController::updateControlledObjects warning: could not update tracking info.";
+ return;
+ }
+ }
+
+ Vec3d p = m_trackingController->getPosition();
+ Quatd r = m_trackingController->getRotation();
+
+ // Update jaw angles
+ if (m_trackingController->getDeviceClient()->getButton(0))
+ {
+ m_jawAngle += m_change;
+ m_jawAngle = (m_jawAngle > m_maxJawAngle) ? m_maxJawAngle : m_jawAngle;
+ }
+
+ if (m_trackingController->getDeviceClient()->getButton(1))
+ {
+ m_jawAngle -= m_change;
+ m_jawAngle = (m_jawAngle < 0.0) ? 0.0 : m_jawAngle;
+ }
+
+ // Update orientation of parts
+ Quatd jawRotUpper;
+ jawRotUpper = r*Rotd(m_jawAngle, m_jawRotationAxis);
+ m_upperJaw->getMasterGeometry()->setOrientation(jawRotUpper);
+
+ Quatd jawRotLower;
+ jawRotLower = r*Rotd(-m_jawAngle, m_jawRotationAxis);
+ m_lowerJaw->getMasterGeometry()->setOrientation(jawRotLower);
+
+ m_shaft->getMasterGeometry()->setOrientation(r);
+
+ // Update positions of parts
+ m_shaft->getMasterGeometry()->setPosition(p);
+ m_upperJaw->getMasterGeometry()->setPosition(p);
+ m_lowerJaw->getMasterGeometry()->setPosition(p);
+}
+
+void
+LaparoscopicToolController::applyForces()
+{
+ Vec3d force(0, 0, 0);
+
+ if (auto collidingObject = dynamic_cast<CollidingObject*>(m_shaft.get()))
+ {
+ force += collidingObject->getForce();
+ }
+
+ if (auto collidingObject = dynamic_cast<CollidingObject*>(m_upperJaw.get()))
+ {
+ force += collidingObject->getForce();
+ }
+
+ if (auto collidingObject = dynamic_cast<CollidingObject*>(m_lowerJaw.get()))
+ {
+ force += collidingObject->getForce();
+ }
+
+ m_trackingController->getDeviceClient()->setForce(force);
+}
+
+} // imstk
diff --git a/Base/SceneElements/Controllers/imstkLaparoscopicToolController.h b/Base/SceneElements/Controllers/imstkLaparoscopicToolController.h
new file mode 100644
index 0000000000000000000000000000000000000000..ac9ad4ac565a13b8db7220cda19644105b03c8ea
--- /dev/null
+++ b/Base/SceneElements/Controllers/imstkLaparoscopicToolController.h
@@ -0,0 +1,134 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0.txt
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+
+ =========================================================================*/
+
+#ifndef imstkLaparoscopicToolController_h
+#define imstkLaparoscopicToolController_h
+
+#include "imstkSceneObjectControllerBase.h"
+#include "imstkDeviceTracker.h"
+#include "imstkSceneObject.h"
+#include "imstkMath.h"
+
+#include <memory>
+
+namespace imstk
+{
+
+///
+/// \class LaparoscopicTool
+///
+/// \brief Two-jawed laparoscopic tool controlled by external device
+/// The tool is composed of three scene objects: pivot, lower jaw and upper jaw
+/// The jaws open-close based on the buttons at present.
+/// This has to be replaced by potentiometer tracking in future.
+///
+class LaparoscopicToolController : public SceneObjectControllerBase
+{
+public:
+ ///
+ /// \brief Constructor
+ ///
+ LaparoscopicToolController(
+ std::shared_ptr<SceneObject> shaft,
+ std::shared_ptr<SceneObject> upperJaw,
+ std::shared_ptr<SceneObject> lowerJaw,
+ std::shared_ptr<DeviceTracker> trackingController) :
+ m_trackingController(trackingController),
+ m_shaft(shaft),
+ m_upperJaw(upperJaw),
+ m_lowerJaw(lowerJaw) {}
+
+ LaparoscopicToolController() = delete; //not allowed for now
+
+ ///
+ /// \brief Destructor
+ ///
+ ~LaparoscopicToolController() = default;
+
+ ///
+ /// \brief Initialize offset based on object geometry
+ ///
+ void initOffsets() override;
+
+ ///
+ /// \brief Update controlled laparoscopic tool using latest tracking information
+ ///
+ void updateControlledObjects() override;
+
+ ///
+ /// \brief Apply forces to the haptic device
+ ///
+ void applyForces() override;
+
+ ///
+ /// \brief Set the tracker to out-of-date
+ ///
+ inline void setTrackerToOutOfDate() override { m_trackingController->setTrackerToOutOfDate(); }
+
+ ///
+ /// \brief Set the maximum jaw angle
+ ///
+ inline void setMaxJawAngle(const double maxAngle) { m_maxJawAngle = maxAngle; }
+
+ ///
+ /// \brief Set the increment
+ ///
+ inline void setJawAngleChange(const double dAngle) { m_change = dAngle; }
+
+ ///
+ /// \brief Set the jaw rotation axis
+ ///
+ inline void setJawRotationAxis(const Vec3d& axis) { m_jawRotationAxis = axis; }
+
+ ///
+ /// \brief Get the current jaw angle
+ ///
+ inline double getJawAngle() const { return m_jawAngle; }
+
+ ///
+ /// \brief Get the max jaw angle
+ ///
+ inline double getMaxJawAngle() const { return m_maxJawAngle; }
+
+ ///
+ /// \brief Get/Set tracking controller
+ ///
+ inline std::shared_ptr<DeviceTracker> getTrackingController() const { return m_trackingController; }
+ inline void setTrackingController(std::shared_ptr<DeviceTracker> controller) { m_trackingController = controller; }
+
+protected:
+
+ std::shared_ptr<DeviceTracker> m_trackingController; ///< Device tracker
+
+ std::shared_ptr<SceneObject> m_shaft; ///< Tool shaft
+ std::shared_ptr<SceneObject> m_upperJaw; ///< Tool upper jaw
+ std::shared_ptr<SceneObject> m_lowerJaw; ///< Tool lower jaw
+
+ double m_jawAngle = PI / 6.0; ///< Angle of the jaws
+ double m_change = 6.0e-5; ///< Amount of change in jaw angle per frame
+ double m_maxJawAngle = PI / 6.0; ///< Maximum angle of the jaws
+
+ Vec3d m_jawRotationAxis = Vec3d(0, 1., 0); ///< Angle of the jaws
+};
+
+} // imstk
+
+#endif // ifndef imstkLaparoscopicToolController_h
diff --git a/Base/SceneElements/Controllers/imstkSceneObjectController.cpp b/Base/SceneElements/Controllers/imstkSceneObjectController.cpp
index c4ed02636e1caa957d848e5b30953b9498d80ff5..2053c57c50ea7c7d12f3b0bdb253d64b8d3b1842 100644
--- a/Base/SceneElements/Controllers/imstkSceneObjectController.cpp
+++ b/Base/SceneElements/Controllers/imstkSceneObjectController.cpp
@@ -34,33 +34,33 @@ namespace imstk
void
SceneObjectController::initOffsets()
{
- m_translationOffset = m_sceneObject.getMasterGeometry()->getPosition();
- m_rotationOffset = m_sceneObject.getMasterGeometry()->getOrientation();
+ m_trackingController->setTranslationOffset(m_sceneObject->getMasterGeometry()->getPosition());
+ m_trackingController->setRotationOffset(m_sceneObject->getMasterGeometry()->getOrientation());
}
void
-SceneObjectController::updateFromDevice()
+SceneObjectController::updateControlledObjects()
{
- Vec3d p;
- Quatd r;
-
- if (!this->computeTrackingData(p, r))
+ if (!m_trackingController->isTrackerUpToDate())
{
- LOG(WARNING) << "SceneObjectController::updateFromDevice warning: could not update tracking info.";
- return;
+ if (!m_trackingController->updateTrackingData())
+ {
+ LOG(WARNING) << "SceneObjectController::updateControlledObjects warning: could not update tracking info.";
+ return;
+ }
}
// Update colliding geometry
- m_sceneObject.getMasterGeometry()->setPosition(p);
- m_sceneObject.getMasterGeometry()->setOrientation(r);
+ m_sceneObject->getMasterGeometry()->setPosition(m_trackingController->getPosition());
+ m_sceneObject->getMasterGeometry()->setOrientation(m_trackingController->getRotation());
}
void
SceneObjectController::applyForces()
{
- if(auto collidingObject = dynamic_cast<CollidingObject*>(&m_sceneObject))
+ if(auto collidingObject = dynamic_cast<CollidingObject*>(m_sceneObject.get()))
{
- m_deviceClient->setForce(collidingObject->getForce());
+ m_trackingController->getDeviceClient()->setForce(collidingObject->getForce());
}
}
diff --git a/Base/SceneElements/Controllers/imstkSceneObjectController.h b/Base/SceneElements/Controllers/imstkSceneObjectController.h
index 2563c4292ba1e508fc03112be3eb69fd9a731247..2357d7e322622b8121352bb897276cf58513cedd 100644
--- a/Base/SceneElements/Controllers/imstkSceneObjectController.h
+++ b/Base/SceneElements/Controllers/imstkSceneObjectController.h
@@ -22,7 +22,8 @@
#ifndef imstkSceneObjectController_h
#define imstkSceneObjectController_h
-#include "imstkTrackingController.h"
+#include "imstkSceneObjectControllerBase.h"
+#include "imstkDeviceTracker.h"
#include "imstkSceneObject.h"
#include <memory>
@@ -33,19 +34,18 @@ namespace imstk
///
/// \class SceneObjectController
///
-/// \brief
+/// \brief This class implements once tracking controller controlling one scnene object
///
-class SceneObjectController : public TrackingController
+class SceneObjectController : public SceneObjectControllerBase
{
public:
///
/// \brief Constructor
///
- SceneObjectController(SceneObject& sceneObject,
- std::shared_ptr<DeviceClient> deviceClient) :
- TrackingController(deviceClient),
- m_sceneObject(sceneObject)
- {}
+ SceneObjectController(std::shared_ptr<SceneObject> sceneObject, std::shared_ptr<DeviceTracker> trackingController) :
+ m_trackingController(trackingController), m_sceneObject(sceneObject) {}
+
+ SceneObjectController() = delete;
///
/// \brief Destructor
@@ -55,22 +55,38 @@ public:
///
/// \brief Initialize offset based on object geometry
///
- void initOffsets();
+ void initOffsets() override;
///
- /// \brief Update geometries transformations
+ /// \brief Update controlled scene object using latest tracking information
///
- void updateFromDevice();
+ void updateControlledObjects() override;
///
/// \brief Apply forces to the haptic device
///
- void applyForces();
+ void applyForces() override;
-protected:
+ ///
+ /// \brief Sets the tracker to out-of-date
+ ///
+ inline void setTrackerToOutOfDate() override { m_trackingController->setTrackerToOutOfDate(); }
+
+ ///
+ /// \brief Get/Set controlled scene object
+ ///
+ inline std::shared_ptr<SceneObject> getControlledSceneObject() const { return m_sceneObject; }
+ inline void setControlledSceneObject(std::shared_ptr<SceneObject> so) { m_sceneObject = so; }
- SceneObject& m_sceneObject; ///< SceneObject controlled by the external device
+ ///
+ /// \brief Get/Set tracking controller
+ ///
+ inline std::shared_ptr<DeviceTracker> getTrackingController() const { return m_trackingController; }
+ inline void setTrackingController(std::shared_ptr<DeviceTracker> controller) { m_trackingController = controller; }
+protected:
+ std::shared_ptr<DeviceTracker> m_trackingController; ///< Device tracker
+ std::shared_ptr<SceneObject> m_sceneObject; ///< SceneObject controlled by the Tracker
};
} // imstk
diff --git a/Base/SceneElements/Controllers/imstkSceneObjectControllerBase.h b/Base/SceneElements/Controllers/imstkSceneObjectControllerBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..4fc3d6740e9311c6bc0a53e9d976c5a270e14840
--- /dev/null
+++ b/Base/SceneElements/Controllers/imstkSceneObjectControllerBase.h
@@ -0,0 +1,65 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0.txt
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+
+ =========================================================================*/
+
+#ifndef imstkSceneObjectControllerBase_h
+#define imstkSceneObjectControllerBase_h
+
+namespace imstk
+{
+
+///
+/// \class SceneObjectControllerBase
+///
+/// \brief Base class for all the scene object controllers
+///
+class SceneObjectControllerBase
+{
+public:
+ ///
+ /// \brief Constructor/Destructor
+ ///
+ SceneObjectControllerBase() = default;
+ ~SceneObjectControllerBase() = default;
+
+ ///
+ /// \brief Initialize offset based on object geometry
+ ///
+ virtual void initOffsets() = 0;
+
+ ///
+ /// \brief Update controlled scene objects using latest tracking information
+ ///
+ virtual void updateControlledObjects() = 0;
+
+ ///
+ /// \brief Apply forces to the haptic device
+ ///
+ virtual void applyForces() = 0;
+
+ ///
+ /// \brief
+ ///
+ virtual void setTrackerToOutOfDate() = 0;
+};
+
+} // imstk
+
+#endif // ifndef imstkSceneObjectControllerBase_h
\ No newline at end of file
diff --git a/Base/SceneElements/Objects/imstkSceneObject.cpp b/Base/SceneElements/Objects/imstkSceneObject.cpp
index e25bea4d995f0cc9a4661c9fd742035034646b70..8ab778dc4a200747969113150973096ecf8d32ca 100644
--- a/Base/SceneElements/Objects/imstkSceneObject.cpp
+++ b/Base/SceneElements/Objects/imstkSceneObject.cpp
@@ -69,24 +69,4 @@ SceneObject::setName(const std::string& name)
m_name = name;
}
-std::shared_ptr<SceneObjectController>
-SceneObject::getController() const
-{
- return m_controller;
-}
-
-std::shared_ptr<SceneObjectController>
-SceneObject::setupController(std::shared_ptr<DeviceClient> deviceClient)
-{
- if(m_controller == nullptr)
- {
- m_controller = std::make_shared<SceneObjectController>(*this, deviceClient);
- }
- else
- {
- m_controller->setDeviceClient(deviceClient);
- }
- return m_controller;
-}
-
} // imstk
diff --git a/Base/SceneElements/Objects/imstkSceneObject.h b/Base/SceneElements/Objects/imstkSceneObject.h
index 38045907bad200c5b6056d7420d8bc6075bab324..229b1356c4b16eac5897c233df5333f2799f00cd 100644
--- a/Base/SceneElements/Objects/imstkSceneObject.h
+++ b/Base/SceneElements/Objects/imstkSceneObject.h
@@ -29,7 +29,6 @@ namespace imstk
{
class Geometry;
-class SceneObjectController;
class DeviceClient;
///
@@ -85,16 +84,6 @@ public:
///
virtual std::shared_ptr<Geometry> getMasterGeometry() const;
- ///
- /// \brief Get the object controller
- ///
- std::shared_ptr<SceneObjectController> getController() const;
-
- ///
- /// \brief Setup a controller for the object for a given device client
- ///
- std::shared_ptr<SceneObjectController> setupController(std::shared_ptr<DeviceClient> deviceClient);
-
///
/// \brief
///
@@ -109,7 +98,6 @@ protected:
Type m_type; ///> Type of the scene object
std::string m_name; ///> Custom name of the scene object
std::shared_ptr<Geometry> m_visualGeometry; ///> Geometry for rendering
- std::shared_ptr<SceneObjectController> m_controller; ///> Object controller
};
using VisualObject = SceneObject;
diff --git a/Base/SceneElements/Objects/imstkVirtualCouplingPBDObject.cpp b/Base/SceneElements/Objects/imstkVirtualCouplingPBDObject.cpp
index b1f2f8f2a7e1be1b10aaaef25c81ee7e647ea23b..eb57f013c7b9b2d0c25570880fe8c32e9586bcd4 100644
--- a/Base/SceneElements/Objects/imstkVirtualCouplingPBDObject.cpp
+++ b/Base/SceneElements/Objects/imstkVirtualCouplingPBDObject.cpp
@@ -40,45 +40,49 @@ VirtualCouplingPBDObject::initOffsets()
void
VirtualCouplingPBDObject::updateFromDevice()
{
- Vec3d p;
- Quatd r;
- if (!this->computeTrackingData(p, r))
+ if (!m_trackingDataUptoDate)
{
- LOG(WARNING) << "VirtualCouplingPBDObject::updateFromDevice warning: could not update tracking info.";
- return;
+ if (!updateTrackingData())
+ {
+ LOG(WARNING) << "VirtualCouplingPBDObject::updateFromDevice warning: could not update tracking info.";
+ return;
+ }
}
+ Vec3d p = getPosition();
+ Quatd r = getRotation();
+
// Update colliding geometry
m_visualGeometry->setPosition(p);
m_visualGeometry->setOrientation(r);
computeTransform(p, r, transform);
- auto collidingMesh = std::dynamic_pointer_cast<Mesh>(m_collidingGeometry);
+ auto collidingMesh = std::dynamic_pointer_cast<Mesh>(m_collidingGeometry);
- Vec4d vertexPos4;
- vertexPos4.w() = 1;
- Vec3d vertexPos3;
+ Vec4d vertexPos4;
+ vertexPos4.w() = 1;
+ Vec3d vertexPos3;
for (int i = 0; i < collidingMesh->getNumVertices(); ++i)
- {
- vertexPos3 = collidingMesh->getVertexPosition(i);
- vertexPos4.x() = vertexPos3.x();
- vertexPos4.y() = vertexPos3.y();
- vertexPos4.z() = vertexPos3.z();
-
- vertexPos4.applyOnTheLeft(transform);
- vertexPos3.x() = vertexPos4.x();
- vertexPos3.y() = vertexPos4.y();
- vertexPos3.z() = vertexPos4.z();
-
- collidingMesh->setVerticePosition(i, vertexPos3);
+ {
+ vertexPos3 = collidingMesh->getVertexPosition(i);
+ vertexPos4.x() = vertexPos3.x();
+ vertexPos4.y() = vertexPos3.y();
+ vertexPos4.z() = vertexPos3.z();
+
+ vertexPos4.applyOnTheLeft(transform);
+ vertexPos3.x() = vertexPos4.x();
+ vertexPos3.y() = vertexPos4.y();
+ vertexPos3.z() = vertexPos4.z();
+
+ collidingMesh->setVerticePosition(i, vertexPos3);
}
applyCollidingToPhysics();
updatePbdStates();
}
-void
+void
VirtualCouplingPBDObject::computeTransform(Vec3d& pos, Quatd& quat, Eigen::Matrix4d& t){
auto scaling = m_collidingGeometry->getScaling();
auto angleAxis = Rotd(quat);
diff --git a/Base/SceneElements/Objects/imstkVirtualCouplingPBDObject.h b/Base/SceneElements/Objects/imstkVirtualCouplingPBDObject.h
index b959cdb51cf8f803bb72db832b59bd1852ad1f07..087cd90ff6fc4e57a4ed9180af0b94a9f8d050b5 100644
--- a/Base/SceneElements/Objects/imstkVirtualCouplingPBDObject.h
+++ b/Base/SceneElements/Objects/imstkVirtualCouplingPBDObject.h
@@ -22,7 +22,7 @@
#ifndef imstkVirtualCouplingPBDObject_h
#define imstkVirtualCouplingPBDObject_h
-#include "imstkTrackingController.h"
+#include "imstkDeviceTracker.h"
#include "imstkPbdRigidObject.h"
namespace imstk
{
@@ -35,7 +35,7 @@ class GeometryMap;
///
/// \brief
///
-class VirtualCouplingPBDObject : public TrackingController, public PbdRigidObject
+class VirtualCouplingPBDObject : public DeviceTracker, public PbdRigidObject
{
public:
///
@@ -43,7 +43,7 @@ public:
///
VirtualCouplingPBDObject(std::string name,
std::shared_ptr<DeviceClient> deviceClient) :
- TrackingController(deviceClient),
+ DeviceTracker(deviceClient),
PbdRigidObject(name)
{
}
@@ -96,6 +96,11 @@ public:
void applyCollidingToPhysics();
+ ///
+ /// \brief Set the virtual coupling tracker to out-of-date
+ ///
+ void setTrackerToOutOfDate() { DeviceTracker::setTrackerToOutOfDate(); };
+
protected:
bool m_forceModified;
diff --git a/Base/SimulationManager/imstkSceneManager.cpp b/Base/SimulationManager/imstkSceneManager.cpp
index 94a5ce8da9353da6714c8e8b0eb0470c5dbf7177..f8f7f182b2c5e6eb2505f7a0b6d7b82f88fad40e 100644
--- a/Base/SimulationManager/imstkSceneManager.cpp
+++ b/Base/SimulationManager/imstkSceneManager.cpp
@@ -49,13 +49,15 @@ SceneManager::initModule()
this->startModuleInNewThread(camController);
}
+ // Update objects controlled by the device controllers
+ for (auto controller : m_scene->getSceneObjectControllers())
+ {
+ controller->initOffsets();
+ }
+
// Init virtual coupling objects offsets
for (auto obj : m_scene->getSceneObjects())
{
- if (auto controller = obj->getController())
- {
- controller->initOffsets();
- }
if (auto virtualCouplingPBD = std::dynamic_pointer_cast<VirtualCouplingPBDObject>(obj))
{
virtualCouplingPBD->initOffsets();
@@ -66,27 +68,26 @@ SceneManager::initModule()
void
SceneManager::runModule()
{
- // Update virtualCoupling objects based on devices
+ // Reset Colliding Geometry so that the transform obtained from device can be applied
for (auto obj : m_scene->getSceneObjects())
{
- if (auto controller = obj->getController())
+ if (auto collidingObj = std::dynamic_pointer_cast<CollidingObject>(obj))
{
- controller->updateFromDevice();
- if (auto collidingObj = std::dynamic_pointer_cast<CollidingObject>(obj))
- {
- controller->applyForces();
- collidingObj->setForce(Vec3d(0,0,0));
- }
+ collidingObj->setForce(Vec3d(0,0,0));
}
- else if (auto virtualCouplingPBD = std::dynamic_pointer_cast<VirtualCouplingPBDObject>(obj))
+ if (auto virtualCouplingPBD = std::dynamic_pointer_cast<VirtualCouplingPBDObject>(obj))
{
- // reset Colliding Geometry so that the transform obtained from device can be applied
virtualCouplingPBD->resetCollidingGeometry();
- virtualCouplingPBD->updateFromDevice();
- virtualCouplingPBD->applyForces();
}
}
+ // Update objects controlled by the device controllers
+ for (auto controller : m_scene->getSceneObjectControllers())
+ {
+ controller->updateControlledObjects();
+ controller->applyForces();
+ }
+
// Compute collision data per interaction pair
for (auto intPair : m_scene->getCollisionGraph()->getInteractionPairList())
{
@@ -118,6 +119,21 @@ SceneManager::runModule()
}
intPair->resolveCollision();
}
+
+ // Set the trackers of virtual coupling PBD objects to out-of-date
+ for (auto obj : m_scene->getSceneObjects())
+ {
+ if (auto virtualCouplingPBD = std::dynamic_pointer_cast<VirtualCouplingPBDObject>(obj))
+ {
+ virtualCouplingPBD->setTrackerToOutOfDate();
+ }
+ }
+
+ // Set the trackers of the scene object controllers to out-of-date
+ for (auto controller : m_scene->getSceneObjectControllers())
+ {
+ controller->setTrackerToOutOfDate();
+ }
}
void
diff --git a/Base/SimulationManager/imstkSimulationManager.cpp b/Base/SimulationManager/imstkSimulationManager.cpp
index a03cd9624882177bc787e4f686c07a1eea4491e4..e5829d37c88805dd21bfef3e821175e28d9b6504 100644
--- a/Base/SimulationManager/imstkSimulationManager.cpp
+++ b/Base/SimulationManager/imstkSimulationManager.cpp
@@ -170,6 +170,12 @@ SimulationManager::getViewer() const
return m_viewer;
}
+void
+SimulationManager::setCurrentScene(std::shared_ptr<Scene> scene, bool unloadCurrentScene)
+{
+ setCurrentScene(scene->getName(), unloadCurrentScene);
+}
+
void
SimulationManager::setCurrentScene(std::string newSceneName, bool unloadCurrentScene)
{
diff --git a/Base/SimulationManager/imstkSimulationManager.h b/Base/SimulationManager/imstkSimulationManager.h
index 413a48c158d7167de5a3ec8b26eeca83cb70ca40..e8edc79f391cbc8b5141682e59e006d7ecb862bf 100644
--- a/Base/SimulationManager/imstkSimulationManager.h
+++ b/Base/SimulationManager/imstkSimulationManager.h
@@ -133,6 +133,7 @@ public:
/// \brief
///
void setCurrentScene(std::string newSceneName, bool unloadCurrentScene = false);
+ void setCurrentScene(std::shared_ptr<Scene> scene, bool unloadCurrentScene = false);
///
/// \brief
diff --git a/Examples/Sandbox/main.cpp b/Examples/Sandbox/main.cpp
index 0bc71fbf42027b5eb3e5da32b13ed305e21bd941..df023ae1b8f4fc046821f7ba549bfe56c296c91f 100644
--- a/Examples/Sandbox/main.cpp
+++ b/Examples/Sandbox/main.cpp
@@ -53,6 +53,7 @@
#include "imstkVRPNDeviceServer.h"
#include "imstkCameraController.h"
#include "imstkSceneObjectController.h"
+#include "imstkLaparoscopicToolController.h"
// Collisions
#include "imstkInteractionPair.h"
@@ -102,6 +103,7 @@ void testPbdCloth();
void testPbdCollision();
void testLineMesh();
void testMshAndVegaIO();
+void testLapToolController();
int main()
{
@@ -117,35 +119,133 @@ int main()
<< "Starting Sandbox\n"
<< "****************\n";
+ /*------------------
+ Test rendering
+ ------------------*/
//testMultiTextures();
+ //testVTKTexture();
+ //testMultiObjectWithTextures();
+ //testViewer();
+
+
+ /*------------------
+ Test CD and CR
+ ------------------*/
//testMeshCCD();
//testPenaltyRigidCollision();
- //testTwoFalcons();
- //testObjectController();
- //testCameraController();
- //testViewer();
- //testReadMesh();
- //testAnalyticalGeometry();
- //testScenesManagement();
+
+
+ /*------------------
+ Test geometry, maps
+ ------------------*/
//testIsometricMap();
//testTetraTriangleMap();
//testExtractSurfaceMesh();
//testOneToOneNodalMap();
//testSurfaceMeshOptimizer();
- testDeformableBody();
- //testVTKTexture();
- //testMultiObjectWithTextures();
- //testTwoOmnis();
- //testVectorPlotters();
+ //testAnalyticalGeometry();
+
+
+ /*------------------
+ Test physics
+ ------------------*/
//testPbdVolume();
//testPbdCloth();
//testPbdCollision();
+ //testDeformableBody();
+
+
+ /*------------------
+ Test mesh I/O
+ ------------------*/
//testLineMesh();
//testMshAndVegaIO();
+ //testReadMesh();
+
+
+ /*------------------
+ Test devices, controllers
+ ------------------*/
+ //testObjectController();
+ //testTwoFalcons();
+ //testCameraController();
+ //testTwoOmnis();
+ testLapToolController();
+
+
+ /*------------------
+ Test Misc.
+ ------------------*/
+ //testScenesManagement();
+ //testVectorPlotters();
+
return 0;
}
+void testLapToolController()
+{
+
+#ifdef iMSTK_USE_OPENHAPTICS
+ // SDK and Scene
+ auto sdk = std::make_shared<imstk::SimulationManager>();
+ auto scene = sdk->createNewScene("TestLapToolController");
+
+ // Device clients
+ auto client0 = std::make_shared<imstk::HDAPIDeviceClient>("PHANToM 1");
+
+ // Device Server
+ auto server = std::make_shared<imstk::HDAPIDeviceServer>();
+ server->addDeviceClient(client0);
+ sdk->addModule(server);
+
+ // Plane
+ auto planeGeom = std::make_shared<imstk::Plane>();
+ planeGeom->scale(100);
+ planeGeom->translate(Vec3d(0., -20., 0.));
+ auto planeObj = std::make_shared<imstk::VisualObject>("VisualPlane");
+ planeObj->setVisualGeometry(planeGeom);
+ scene->addSceneObject(planeObj);
+
+ auto createAndAddVisualSceneObject =
+ [](std::shared_ptr<imstk::Scene> scene,
+ const std::string& fileName,
+ const std::string& objectName) ->
+ std::shared_ptr<imstk::SceneObject>
+ {
+ auto mesh = imstk::MeshIO::read(fileName);
+ auto SurfaceMesh = std::dynamic_pointer_cast<imstk::SurfaceMesh>(mesh);
+
+ // Create object and add to scene
+ auto meshObject = std::make_shared<imstk::VisualObject>("meshObject");
+ meshObject->setVisualGeometry(SurfaceMesh);
+ meshObject->setName(objectName);
+ scene->addSceneObject(meshObject);
+ return meshObject;
+ };
+
+ // laparoscopic tool
+ auto pivot = createAndAddVisualSceneObject(scene, DATA_ROOT_PATH"/laptool/pivot.obj", "pivot");
+ auto upperJaw = createAndAddVisualSceneObject(scene, DATA_ROOT_PATH"/laptool/upper.obj", "upperJaw");
+ auto lowerJaw = createAndAddVisualSceneObject(scene, DATA_ROOT_PATH"/laptool/lower.obj", "lowerJaw");
+
+ auto trackingCtrl = std::make_shared<imstk::DeviceTracker>(client0);
+ trackingCtrl->setTranslationScaling(0.5);
+ auto lapToolController = std::make_shared<imstk::LaparoscopicToolController>(pivot, upperJaw, lowerJaw, trackingCtrl);
+ lapToolController->setJawRotationAxis(imstk::Vec3d(1.0, 0, 0));
+ scene->addObjectController(lapToolController);
+
+ // Set Camera
+ auto cam = scene->getCamera();
+ cam->setPosition(imstk::Vec3d(0, 30, 60));
+ cam->setFocalPoint(imstk::Vec3d(0, 0, 0));
+
+ // Run
+ sdk->setCurrentScene(scene);
+ sdk->startSimulation(true);
+#endif
+}
+
void testMshAndVegaIO()
{
// SDK and Scene
@@ -201,7 +301,7 @@ void testMshAndVegaIO()
scene->addSceneObject(objectB);
// Run
- sdk->setCurrentScene("SceneTestMesh");
+ sdk->setCurrentScene(scene);
sdk->startSimulation(true);
}
@@ -313,7 +413,7 @@ void testMultiObjectWithTextures()
scene->addSceneObject(object1);
}
// Run
- sdk->setCurrentScene("multiObjectWithTexturesTest");
+ sdk->setCurrentScene(scene);
sdk->startSimulation(true);
}
@@ -338,7 +438,7 @@ void testMultiTextures()
scene->addSceneObject(object);
// Run
- sdk->setCurrentScene("multitexturestest");
+ sdk->setCurrentScene(scene);
sdk->startSimulation(true);
}
@@ -384,7 +484,7 @@ void testMeshCCD()
});
// Run
- sdk->setCurrentScene("MeshCCDTest");
+ sdk->setCurrentScene(scene);
sdk->startSimulation(true);
t.join();
}
@@ -424,10 +524,13 @@ void testPenaltyRigidCollision()
auto sphere0Obj = std::make_shared<imstk::CollidingObject>("Sphere0");
sphere0Obj->setVisualGeometry(sphere0Geom);
sphere0Obj->setCollidingGeometry(sphere0Geom);
- auto sphere0Controller = sphere0Obj->setupController(client0);
- sphere0Controller->setTranslationScaling(40);
scene->addSceneObject(sphere0Obj);
+ auto trackCtrl0 = std::make_shared<imstk::DeviceTracker>(client0);
+ trackCtrl0->setTranslationScaling(40);
+ auto sphere0Controller = std::make_shared<imstk::SceneObjectController>(sphere0Obj, trackCtrl0);
+ scene->addObjectController(sphere0Controller);
+
// Sphere1
auto sphere1Geom = std::make_shared<Sphere>();
sphere1Geom->scale(0.5);
@@ -435,10 +538,13 @@ void testPenaltyRigidCollision()
auto sphere1Obj = std::make_shared<imstk::CollidingObject>("Sphere1");
sphere1Obj->setVisualGeometry(sphere1Geom);
sphere1Obj->setCollidingGeometry(sphere1Geom);
- auto sphere1Controller = sphere1Obj->setupController(client1);
- sphere1Controller->setTranslationScaling(40);
scene->addSceneObject(sphere1Obj);
+ auto trackCtrl1 = std::make_shared<imstk::DeviceTracker>(client1);
+ trackCtrl1->setTranslationScaling(40);
+ auto sphere1Controller = std::make_shared<imstk::SceneObjectController>(sphere1Obj, trackCtrl1);
+ scene->addObjectController(sphere1Controller);
+
// Collisions
auto colGraph = scene->getCollisionGraph();
colGraph->addInteractionPair(planeObj, sphere0Obj,
@@ -455,7 +561,7 @@ void testPenaltyRigidCollision()
CollisionHandling::Type::Penalty);
// Run
- sdk->setCurrentScene("InteractionPairTest");
+ sdk->setCurrentScene(scene);
sdk->startSimulation(true);
}
@@ -497,10 +603,13 @@ void testTwoFalcons()
auto sphere0Obj = std::make_shared<imstk::CollidingObject>("Sphere0");
sphere0Obj->setVisualGeometry(sphere0Geom);
sphere0Obj->setCollidingGeometry(sphere0Geom);
- auto sphere0Controller = sphere0Obj->setupController(falcon0);
- sphere0Controller->setTranslationScaling(30);
scene->addSceneObject(sphere0Obj);
+ auto trackCtrl0 = std::make_shared<imstk::DeviceTracker>(falcon0);
+ trackCtrl0->setTranslationScaling(30);
+ auto controller0 = std::make_shared<imstk::SceneObjectController>(sphere0Obj, trackCtrl0);
+ scene->addObjectController(controller0);
+
// Sphere1
auto sphere1Geom = std::make_shared<imstk::Sphere>();
sphere1Geom->setPosition(imstk::Vec3d(-16, 4.5, 0));
@@ -508,10 +617,13 @@ void testTwoFalcons()
auto sphere1Obj = std::make_shared<imstk::CollidingObject>("Sphere1");
sphere1Obj->setVisualGeometry(sphere1Geom);
sphere1Obj->setCollidingGeometry(sphere1Geom);
- auto sphere1Controller = sphere1Obj->setupController(falcon1);
- sphere1Controller->setTranslationScaling(30);
scene->addSceneObject(sphere1Obj);
+ auto trackCtrl1 = std::make_shared<imstk::DeviceTracker>(falcon1);
+ trackCtrl1->setTranslationScaling(30);
+ auto controller1 = std::make_shared<imstk::SceneObjectController>(sphere1Obj, trackCtrl1);
+ scene->addObjectController(controller1);
+
// Camera
auto cam = scene->getCamera();
cam->setPosition(imstk::Vec3d(0, 18, 20));
@@ -521,11 +633,12 @@ void testTwoFalcons()
imstk::CameraController::InvertFlag::rotZ);
// Run
- sdk->setCurrentScene("FalconsTestScene");
+ sdk->setCurrentScene(scene);
sdk->startSimulation(true);
}
-void testTwoOmnis(){
+void testTwoOmnis()
+{
#ifdef iMSTK_USE_OPENHAPTICS
// SDK and Scene
auto sdk = std::make_shared<imstk::SimulationManager>();
@@ -553,31 +666,39 @@ void testTwoOmnis(){
auto sphere0Geom = std::make_shared<imstk::Sphere>();
sphere0Geom->setPosition(imstk::Vec3d(2, 2.5, 0));
sphere0Geom->scale(1);
+
auto sphere0Obj = std::make_shared<imstk::CollidingObject>("Sphere0");
sphere0Obj->setVisualGeometry(sphere0Geom);
sphere0Obj->setCollidingGeometry(sphere0Geom);
- auto sphere0Controller = sphere0Obj->setupController(client0);
- sphere0Controller->setTranslationScaling(0.05);
scene->addSceneObject(sphere0Obj);
+ auto trackCtrl0 = std::make_shared<imstk::DeviceTracker>(client0);
+ trackCtrl0->setTranslationScaling(0.05);
+ auto controller0 = std::make_shared<imstk::SceneObjectController>(sphere0Obj, trackCtrl0);
+ scene->addObjectController(controller0);
+
// Sphere1
auto sphere1Geom = std::make_shared<imstk::Sphere>();
sphere1Geom->setPosition(imstk::Vec3d(-2, 2.5, 0));
sphere1Geom->scale(1);
+
auto sphere1Obj = std::make_shared<imstk::CollidingObject>("Sphere1");
sphere1Obj->setVisualGeometry(sphere1Geom);
sphere1Obj->setCollidingGeometry(sphere1Geom);
- auto sphere1Controller = sphere1Obj->setupController(client1);
- sphere1Controller->setTranslationScaling(0.05);
scene->addSceneObject(sphere1Obj);
+ auto trackCtrl1 = std::make_shared<imstk::DeviceTracker>(client1);
+ trackCtrl1->setTranslationScaling(0.05);
+ auto controller1 = std::make_shared<imstk::SceneObjectController>(sphere1Obj, trackCtrl1);
+ scene->addObjectController(controller1);
+
// Update Camera position
auto cam = scene->getCamera();
cam->setPosition(imstk::Vec3d(0, 0, 10));
cam->setFocalPoint(sphere0Geom->getPosition());
// Run
- sdk->setCurrentScene("OmnisTestScene");
+ sdk->setCurrentScene(scene);
sdk->startSimulation(false);
#endif
}
@@ -601,20 +722,24 @@ void testObjectController()
auto geom = std::make_shared<imstk::Cube>();
geom->setPosition(imstk::UP_VECTOR);
geom->scale(2);
+
auto object = std::make_shared<imstk::CollidingObject>("VirtualObject");
object->setVisualGeometry(geom);
object->setCollidingGeometry(geom);
- auto controller = object->setupController(client);
- controller->setTranslationScaling(0.1);
scene->addSceneObject(object);
+ auto trackCtrl = std::make_shared<imstk::DeviceTracker>(client);
+ trackCtrl->setTranslationScaling(0.1);
+ auto controller = std::make_shared<imstk::SceneObjectController>(object, trackCtrl);
+ scene->addObjectController(controller);
+
// Update Camera position
auto cam = scene->getCamera();
cam->setPosition(imstk::Vec3d(0, 0, 10));
cam->setFocalPoint(geom->getPosition());
// Run
- sdk->setCurrentScene("SceneTestDevice");
+ sdk->setCurrentScene(scene);
sdk->startSimulation(false);
#endif
}
@@ -653,7 +778,7 @@ void testCameraController()
imstk::CameraController::InvertFlag::rotZ);
// Run
- sdk->setCurrentScene("SceneTestDevice");
+ sdk->setCurrentScene(scene);
sdk->startSimulation(true);
}
@@ -685,7 +810,7 @@ void testReadMesh()
scene->addSceneObject(object);
// Run
- sdk->setCurrentScene("SceneTestMesh");
+ sdk->setCurrentScene(scene);
sdk->startSimulation(true);
}
@@ -743,7 +868,7 @@ void testViewer()
cam1->setFocalPoint(imstk::Vec3d(1, 1, 0));
// Run
- sdk->setCurrentScene("SceneTest");
+ sdk->setCurrentScene(sceneTest);
sdk->startSimulation(true);
}
@@ -803,18 +928,18 @@ void testScenesManagement()
// switch
LOG(INFO) << "-- Test scene switch";
int delay = 5;
- sdk->setCurrentScene("scene1");
+ sdk->setCurrentScene(scene1);
sdk->startSimulation();
std::this_thread::sleep_for(std::chrono::seconds(delay));
- sdk->setCurrentScene("scene2", false);
+ sdk->setCurrentScene(scene2, false);
std::this_thread::sleep_for(std::chrono::seconds(delay));
- sdk->setCurrentScene("scene1", true);
+ sdk->setCurrentScene(scene1, true);
std::this_thread::sleep_for(std::chrono::seconds(delay));
sdk->endSimulation();
// pause/run
LOG(INFO) << "-- Test simulation pause/run";
- sdk->setCurrentScene("scene2");
+ sdk->setCurrentScene(scene2);
sdk->startSimulation();
std::this_thread::sleep_for(std::chrono::seconds(delay));
sdk->pauseSimulation();
@@ -872,7 +997,7 @@ void testIsometricMap()
LOG(INFO) << cubeGeom->getPosition();
// Start simulation
- sdk->setCurrentScene("geometryMapTest");
+ sdk->setCurrentScene(geometryMapTest);
sdk->startSimulation(imstk::VTKRenderer::Mode::DEBUG);
}
@@ -1191,7 +1316,7 @@ void testDeformableBody()
scene->addNonlinearSolver(nlSolver);
// Run the simulation
- sdk->setCurrentScene("DeformableBodyTest");
+ sdk->setCurrentScene(scene);
sdk->startSimulation(true);
}
@@ -1280,7 +1405,7 @@ void testPbdVolume()
planeObj->setCollidingGeometry(planeGeom);
scene->addSceneObject(planeObj);
- sdk->setCurrentScene("PositionBasedDynamicsTest");
+ sdk->setCurrentScene(scene);
sdk->startSimulation(true);
}
@@ -1370,7 +1495,7 @@ void testPbdCloth()
scene->addLight(colorLight);
scene->addSceneObject(deformableObj);
- sdk->setCurrentScene("PositionBasedDynamicsTest");
+ sdk->setCurrentScene(scene);
sdk->startSimulation(true);
}
@@ -1684,7 +1809,7 @@ void testPbdCollision()
colGraph->addInteractionPair(pair);
}
- sdk->setCurrentScene("PbdCollisionTest");
+ sdk->setCurrentScene(scene);
sdk->startSimulation(true);
}
@@ -2016,7 +2141,7 @@ void testLineMesh()
scene->getCamera()->setFocalPoint(surfMesh.get()->getInitialVertexPosition(20));
}
// Run
- sdk->setCurrentScene("TestLineMesh");
+ sdk->setCurrentScene(scene);
sdk->startSimulation(true);
#endif
}