diff --git a/Examples/PDBPicking/CMakeLists.txt b/Examples/PBDPicking/CMakeLists.txt
similarity index 100%
rename from Examples/PDBPicking/CMakeLists.txt
rename to Examples/PBDPicking/CMakeLists.txt
diff --git a/Examples/PDBPicking/PBDPickingExample.cpp b/Examples/PBDPicking/PBDPickingExample.cpp
similarity index 86%
rename from Examples/PDBPicking/PBDPickingExample.cpp
rename to Examples/PBDPicking/PBDPickingExample.cpp
index f9c655267a487deffe73e0cd52092d68b0b8b7e5..8fdb5802c2f3e9e771ea0fc739b47e847e3a5955 100644
--- a/Examples/PDBPicking/PBDPickingExample.cpp
+++ b/Examples/PBDPicking/PBDPickingExample.cpp
@@ -1,306 +1,312 @@
-/*=========================================================================
-
- 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 "imstkCamera.h"
-#include "imstkCDObjectFactory.h"
-#include "imstkCollidingObject.h"
-#include "imstkCollisionData.h"
-#include "imstkCollisionGraph.h"
-#include "imstkCollisionPair.h"
-#include "imstkCollisionDetection.h"
-#include "imstkHapticDeviceClient.h"
-#include "imstkHapticDeviceManager.h"
-#include "imstkKeyboardDeviceClient.h"
-#include "imstkKeyboardSceneControl.h"
-#include "imstkLight.h"
-#include "imstkMeshIO.h"
-#include "imstkMouseSceneControl.h"
-#include "imstkNew.h"
-#include "imstkPlane.h"
-#include "imstkRenderMaterial.h"
-#include "imstkScene.h"
-#include "imstkSceneManager.h"
-
-#include "imstkLaparoscopicToolController.h"
-#include "imstkSceneObjectController.h"
-
-
-#include "imstkVisualModel.h"
-#include "imstkVTKViewer.h"
-
-#include "imstkLogger.h"
-#include "imstkPbdModel.h"
-#include "imstkPbdObject.h"
-
-#include "imstkSphere.h"
-#include "imstkCapsule.h"
-#include "imstkSurfaceMesh.h"
-
-#include "imstkPBDPickingCH.h"
-#include "imstkPbdObjectPickingPair.h"
-
-#include "imstkIsometricMap.h"
-
-// global variables
-const std::string phantomOmni1Name = "Default Device";
-
-using namespace imstk;
-
-// Parameters to play with
-const double width = 50.0;
-const double height = 50.0;
-const int nRows = 16;
-const int nCols = 16;
-
-///
-/// \brief Creates cloth geometry
-///
-static std::shared_ptr<SurfaceMesh>
-makeClothGeometry(const double width,
- const double height,
- const int nRows,
- const int nCols)
-{
- imstkNew<SurfaceMesh> clothMesh;
-
- StdVectorOfVec3d vertList;
- vertList.resize(nRows * nCols);
- const double dy = width / (double)(nCols - 1);
- const double dx = height / (double)(nRows - 1);
- for (int i = 0; i < nRows; ++i)
- {
- for (int j = 0; j < nCols; j++)
- {
- vertList[i * nCols + j] = Vec3d((double)dx * i, 1.0, (double)dy * j);
- }
- }
- clothMesh->setInitialVertexPositions(vertList);
- clothMesh->setVertexPositions(vertList);
-
- // Add connectivity data
- std::vector<SurfaceMesh::TriangleArray> triangles;
- for (std::size_t i = 0; i < nRows - 1; ++i)
- {
- for (std::size_t j = 0; j < nCols - 1; j++)
- {
- SurfaceMesh::TriangleArray tri[2];
- const size_t index1 = i * nCols + j;
- const size_t index2 = index1 + nCols;
- const size_t index3 = index1 + 1;
- const size_t index4 = index2 + 1;
-
- // Interleave [/][\]
- if (i % 2 ^ j % 2)
- {
- tri[0] = { { index1, index2, index3 } };
- tri[1] = { { index4, index3, index2 } };
- }
- else
- {
- tri[0] = { { index2, index4, index1 } };
- tri[1] = { { index4, index3, index1 } };
- }
- triangles.push_back(tri[0]);
- triangles.push_back(tri[1]);
- }
- }
-
- clothMesh->setTrianglesVertices(triangles);
-
- return clothMesh;
-}
-
-///
-/// \brief Creates cloth object
-///
-static std::shared_ptr<PbdObject>
-makeClothObj(const std::string& name,
- const double width,
- const double height,
- const int nRows,
- const int nCols)
-{
- imstkNew<PbdObject> clothObj(name);
-
- // Setup the Geometry
- std::shared_ptr<SurfaceMesh> clothMesh(makeClothGeometry(width, height, nRows, nCols));
-
- // Setup the Parameters
- imstkNew<PBDModelConfig> pbdParams;
- pbdParams->enableConstraint(PbdConstraint::Type::Distance, 1.0e2);
- pbdParams->enableConstraint(PbdConstraint::Type::Dihedral, 1.0e1);
- pbdParams->m_fixedNodeIds = { 0, static_cast<size_t>(nCols) - 1 };
- pbdParams->m_uniformMassValue = width * height / ((double)nRows * (double)nCols);
- pbdParams->m_gravity = Vec3d(0.0, -9.8, 0.0);
- pbdParams->m_defaultDt = 0.005;
- pbdParams->m_iterations = 5;
-
- // Setup the Model
- imstkNew<PbdModel> pbdModel;
- pbdModel->setModelGeometry(clothMesh);
- pbdModel->configure(pbdParams);
-
- // Setup the VisualModel
- imstkNew<RenderMaterial> material;
- material->setBackFaceCulling(false);
- material->setDisplayMode(RenderMaterial::DisplayMode::WireframeSurface);
-
- imstkNew<VisualModel> visualModel(clothMesh);
- visualModel->setRenderMaterial(material);
-
- // Setup the Object
- clothObj->addVisualModel(visualModel);
- clothObj->setPhysicsGeometry(clothMesh);
- clothObj->setCollidingGeometry(clothMesh);
- clothObj->setDynamicalModel(pbdModel);
-
- return clothObj;
-}
-
-///
-/// \brief This example demonstrates the concept of PBD picking
-/// for haptic interaction. NOTE: Requires GeoMagic Touch device
-///
-int
-main()
-{
- // Setup logger (write to file and stdout)
- Logger::startLogger();
-
- // Scene
- imstkNew<Scene> scene("PBDPicking");
-
- // Create the virtual coupling object controller
-
- // Device Server
- imstkNew<HapticDeviceManager> server;
- std::shared_ptr<HapticDeviceClient> client = server->makeDeviceClient(phantomOmni1Name);
-
- // Load the meshes
- auto upperSurfMesh = MeshIO::read<SurfaceMesh>(iMSTK_DATA_ROOT "/laptool/upper.obj");
- auto lowerSurfMesh = MeshIO::read<SurfaceMesh>(iMSTK_DATA_ROOT "/laptool/lower.obj");
- auto pivotSurfMesh = MeshIO::read<SurfaceMesh>(iMSTK_DATA_ROOT "/laptool/pivot.obj");
-
- imstkNew<Capsule> geomShaft;
- geomShaft->setLength(20.0);
- geomShaft->setRadius(1.0);
- geomShaft->setOrientationAxis(Vec3d(0.0, 0.0, 1.0));
- imstkNew<CollidingObject> objShaft("ShaftObject");
- objShaft->setVisualGeometry(geomShaft);
- objShaft->setCollidingGeometry(geomShaft);
- //objShaft->setCollidingToVisualMap(std::make_shared<IsometricMap>(geomShaft, pivotSurfMesh));
- scene->addSceneObject(objShaft);
-
- imstkNew<Capsule> geomUpperJaw;
- geomUpperJaw->setLength(30.0);
- geomUpperJaw->setPosition(Vec3d(0.0, 0.0, -12.5));
- geomUpperJaw->setRadius(1.0);
- geomUpperJaw->setOrientationAxis(Vec3d(0.0, 0.0, 1.0));
- imstkNew<CollidingObject> objUpperJaw("UpperJawObject");
- objUpperJaw->setVisualGeometry(geomUpperJaw);
- objUpperJaw->setCollidingGeometry(geomUpperJaw);
- /* auto mapUpperJaw = std::make_shared<IsometricMap>(geomUpperJaw, upperSurfMesh);
- objUpperJaw->setCollidingToVisualMap(mapUpperJaw);*/
- scene->addSceneObject(objUpperJaw);
-
- imstkNew<Capsule> geomLowerJaw;
- geomLowerJaw->setLength(30.0);
- geomLowerJaw->setPosition(Vec3d(0.0, 0.0, -12.5));
- geomLowerJaw->setRadius(1.0);
- geomLowerJaw->setOrientationAxis(Vec3d(0.0, 0.0, 1.0));
- imstkNew<CollidingObject> objLowerJaw("LowerJawObject");
- objLowerJaw->setVisualGeometry(geomLowerJaw);
- objLowerJaw->setCollidingGeometry(geomLowerJaw);
- //auto mapLowerJaw = std::make_shared<IsometricMap>(geomLowerJaw, lowerSurfMesh);
- //objLowerJaw->setCollidingToVisualMap(mapLowerJaw);
- scene->addSceneObject(objLowerJaw);
-
- std::shared_ptr<PbdObject> clothObj = makeClothObj("Cloth", width, height, nRows, nCols);
- scene->addSceneObject(clothObj);
-
- // Create and add virtual coupling object controller in the scene
- imstkNew<LaparoscopicToolController> controller(objShaft, objUpperJaw, objLowerJaw, client);
- controller->setJawAngleChange(6.0e-3);
- scene->addController(controller);
-
- // Add interaction pair for pbd picking
- imstkNew<PbdObjectPickingPair> pair(clothObj, objUpperJaw, CollisionDetection::Type::PointSetToCapsule);
- scene->getCollisionGraph()->addInteraction(pair);
-
- // Camera
- scene->getActiveCamera()->setPosition(Vec3d(1, 1, 1) * 100.0);
- scene->getActiveCamera()->setFocalPoint(Vec3d(0, 0, 0));
-
- // Light
- imstkNew<DirectionalLight> light("light");
- light->setFocalPoint(Vec3d(5.0, -8.0, -5.0));
- light->setIntensity(1.0);
- scene->addLight(light);
-
- //Run the simulation
- {
- // Setup a viewer to render in its own thread
- imstkNew<VTKViewer> viewer("Viewer");
- viewer->setActiveScene(scene);
-
- // Setup a scene manager to advance the scene in its own thread
- imstkNew<SceneManager> sceneManager("Scene Manager");
- sceneManager->setActiveScene(scene);
- viewer->addChildThread(sceneManager); // SceneManager will start/stop with viewer
-
- viewer->addChildThread(server);
-
- // Add mouse and keyboard controls to the viewer
- {
- imstkNew<MouseSceneControl> mouseControl(viewer->getMouseDevice());
- mouseControl->setSceneManager(sceneManager);
- viewer->addControl(mouseControl);
-
- imstkNew<KeyboardSceneControl> keyControl(viewer->getKeyboardDevice());
- keyControl->setSceneManager(sceneManager);
- keyControl->setViewer(viewer);
- viewer->addControl(keyControl);
- }
- // Not perfectly thread safe movement lambda, ijkl movement instead of wasd because d is already used
- std::shared_ptr<KeyboardDeviceClient> keyDevice = viewer->getKeyboardDevice();
- connect<Event>(sceneManager, EventType::PreUpdate, [&](Event*)
- {
- std::shared_ptr<PBDPickingCH> ch = std::static_pointer_cast<PBDPickingCH>(pair->getCollisionHandlingA());
- // Activate picking
-
- if (client->getButton(1))
- {
- ch->activatePickConstraints();
- }
- // Unpick
- if (client->getButton(0))
- {
- ch->removePickConstraints();
- }
- });
-
- // Start viewer running, scene as paused
- sceneManager->requestStatus(ThreadStatus::Paused);
- viewer->start();
- }
-
- return 0;
-}
+/*=========================================================================
+
+ 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 "imstkCamera.h"
+#include "imstkCDObjectFactory.h"
+#include "imstkCollidingObject.h"
+#include "imstkCollisionData.h"
+#include "imstkCollisionGraph.h"
+#include "imstkCollisionPair.h"
+#include "imstkCollisionDetection.h"
+#include "imstkHapticDeviceClient.h"
+#include "imstkHapticDeviceManager.h"
+#include "imstkKeyboardDeviceClient.h"
+#include "imstkKeyboardSceneControl.h"
+#include "imstkLight.h"
+#include "imstkMeshIO.h"
+#include "imstkMouseSceneControl.h"
+#include "imstkNew.h"
+#include "imstkPlane.h"
+#include "imstkRenderMaterial.h"
+#include "imstkScene.h"
+#include "imstkSceneManager.h"
+
+#include "imstkLaparoscopicToolController.h"
+#include "imstkSceneObjectController.h"
+
+
+#include "imstkVisualModel.h"
+#include "imstkVTKViewer.h"
+
+#include "imstkLogger.h"
+#include "imstkPbdModel.h"
+#include "imstkPbdObject.h"
+
+#include "imstkSphere.h"
+#include "imstkCapsule.h"
+#include "imstkSurfaceMesh.h"
+
+#include "imstkPBDPickingCH.h"
+#include "imstkPbdObjectPickingPair.h"
+
+#include "imstkIsometricMap.h"
+
+// global variables
+const std::string phantomOmni1Name = "Default Device";
+
+using namespace imstk;
+
+// Parameters to play with
+const double width = 50.0;
+const double height = 50.0;
+const int nRows = 16;
+const int nCols = 16;
+
+///
+/// \brief Creates cloth geometry
+///
+static std::shared_ptr<SurfaceMesh>
+makeClothGeometry(const double width,
+ const double height,
+ const int nRows,
+ const int nCols)
+{
+ imstkNew<SurfaceMesh> clothMesh;
+
+ StdVectorOfVec3d vertList;
+ vertList.resize(nRows * nCols);
+ const double dy = width / (double)(nCols - 1);
+ const double dx = height / (double)(nRows - 1);
+ for (int i = 0; i < nRows; ++i)
+ {
+ for (int j = 0; j < nCols; j++)
+ {
+ vertList[i * nCols + j] = Vec3d((double)dx * i, 1.0, (double)dy * j);
+ }
+ }
+ clothMesh->setInitialVertexPositions(vertList);
+ clothMesh->setVertexPositions(vertList);
+
+ // Add connectivity data
+ std::vector<SurfaceMesh::TriangleArray> triangles;
+ for (std::size_t i = 0; i < nRows - 1; ++i)
+ {
+ for (std::size_t j = 0; j < nCols - 1; j++)
+ {
+ SurfaceMesh::TriangleArray tri[2];
+ const size_t index1 = i * nCols + j;
+ const size_t index2 = index1 + nCols;
+ const size_t index3 = index1 + 1;
+ const size_t index4 = index2 + 1;
+
+ // Interleave [/][\]
+ if (i % 2 ^ j % 2)
+ {
+ tri[0] = { { index1, index2, index3 } };
+ tri[1] = { { index4, index3, index2 } };
+ }
+ else
+ {
+ tri[0] = { { index2, index4, index1 } };
+ tri[1] = { { index4, index3, index1 } };
+ }
+ triangles.push_back(tri[0]);
+ triangles.push_back(tri[1]);
+ }
+ }
+
+ clothMesh->setTrianglesVertices(triangles);
+
+ return clothMesh;
+}
+
+///
+/// \brief Creates cloth object
+///
+static std::shared_ptr<PbdObject>
+makeClothObj(const std::string& name,
+ const double width,
+ const double height,
+ const int nRows,
+ const int nCols)
+{
+ imstkNew<PbdObject> clothObj(name);
+
+ // Setup the Geometry
+ std::shared_ptr<SurfaceMesh> clothMesh(makeClothGeometry(width, height, nRows, nCols));
+
+ // Setup the Parameters
+ imstkNew<PBDModelConfig> pbdParams;
+ pbdParams->enableConstraint(PbdConstraint::Type::Distance, 1.0e2);
+ pbdParams->enableConstraint(PbdConstraint::Type::Dihedral, 1.0e1);
+ pbdParams->m_fixedNodeIds = { 0, static_cast<size_t>(nCols) - 1 };
+ pbdParams->m_uniformMassValue = width * height / ((double)nRows * (double)nCols);
+ pbdParams->m_gravity = Vec3d(0.0, -9.8, 0.0);
+ pbdParams->m_defaultDt = 0.005;
+ pbdParams->m_iterations = 5;
+
+ // Setup the Model
+ imstkNew<PbdModel> pbdModel;
+ pbdModel->setModelGeometry(clothMesh);
+ pbdModel->configure(pbdParams);
+
+ // Setup the VisualModel
+ imstkNew<RenderMaterial> material;
+ material->setBackFaceCulling(false);
+ material->setDisplayMode(RenderMaterial::DisplayMode::WireframeSurface);
+
+ imstkNew<VisualModel> visualModel(clothMesh);
+ visualModel->setRenderMaterial(material);
+
+ // Setup the Object
+ clothObj->addVisualModel(visualModel);
+ clothObj->setPhysicsGeometry(clothMesh);
+ clothObj->setCollidingGeometry(clothMesh);
+ clothObj->setDynamicalModel(pbdModel);
+
+ return clothObj;
+}
+
+///
+/// \brief This example demonstrates the concept of PBD picking
+/// for haptic interaction. NOTE: Requires GeoMagic Touch device
+///
+int
+main()
+{
+ // Setup logger (write to file and stdout)
+ Logger::startLogger();
+
+ // Scene
+ imstkNew<Scene> scene("PBDPicking");
+
+ // Create the virtual coupling object controller
+
+ // Device Server
+ imstkNew<HapticDeviceManager> server;
+ std::shared_ptr<HapticDeviceClient> client = server->makeDeviceClient(phantomOmni1Name);
+
+ // Load the meshes
+ auto upperSurfMesh = MeshIO::read<SurfaceMesh>(iMSTK_DATA_ROOT "/laptool/upper.obj");
+ auto lowerSurfMesh = MeshIO::read<SurfaceMesh>(iMSTK_DATA_ROOT "/laptool/lower.obj");
+ auto pivotSurfMesh = MeshIO::read<SurfaceMesh>(iMSTK_DATA_ROOT "/laptool/pivot.obj");
+
+ imstkNew<Capsule> geomShaft;
+ geomShaft->setLength(20.0);
+ geomShaft->setRadius(1.0);
+ geomShaft->setOrientationAxis(Vec3d(0.0, 0.0, 1.0));
+ geomShaft->setTranslation(Vec3d(0.0, 0.0, 10.0));
+ imstkNew<CollidingObject> objShaft("ShaftObject");
+ objShaft->setVisualGeometry(geomShaft);
+ objShaft->setCollidingGeometry(geomShaft);
+ //objShaft->setCollidingToVisualMap(std::make_shared<IsometricMap>(geomShaft, pivotSurfMesh));
+ scene->addSceneObject(objShaft);
+
+ imstkNew<Capsule> geomUpperJaw;
+ geomUpperJaw->setLength(15.0);
+ geomUpperJaw->setTranslation(Vec3d(0.0, 0.0, -15.0));
+ geomUpperJaw->setRadius(1.0);
+ geomUpperJaw->setOrientationAxis(Vec3d(0.0, 0.0, 1.0));
+ imstkNew<CollidingObject> objUpperJaw("UpperJawObject");
+ objUpperJaw->setVisualGeometry(geomUpperJaw);
+ objUpperJaw->setCollidingGeometry(geomUpperJaw);
+ /* auto mapUpperJaw = std::make_shared<IsometricMap>(geomUpperJaw, upperSurfMesh);
+ objUpperJaw->setCollidingToVisualMap(mapUpperJaw);*/
+ scene->addSceneObject(objUpperJaw);
+
+ imstkNew<Capsule> geomLowerJaw;
+ geomLowerJaw->setLength(15.0);
+ geomLowerJaw->setTranslation(Vec3d(0.0, 0.0, -15.0));
+ geomLowerJaw->setRadius(1.0);
+ geomLowerJaw->setOrientationAxis(Vec3d(0.0, 0.0, 1.0));
+ imstkNew<CollidingObject> objLowerJaw("LowerJawObject");
+ objLowerJaw->setVisualGeometry(geomLowerJaw);
+ objLowerJaw->setCollidingGeometry(geomLowerJaw);
+ //auto mapLowerJaw = std::make_shared<IsometricMap>(geomLowerJaw, lowerSurfMesh);
+ //objLowerJaw->setCollidingToVisualMap(mapLowerJaw);
+ scene->addSceneObject(objLowerJaw);
+
+ std::shared_ptr<PbdObject> clothObj = makeClothObj("Cloth", width, height, nRows, nCols);
+ scene->addSceneObject(clothObj);
+
+ // Create and add virtual coupling object controller in the scene
+ imstkNew<LaparoscopicToolController> controller(objShaft, objUpperJaw, objLowerJaw, client);
+ controller->setJawAngleChange(6.0e-3);
+ scene->addController(controller);
+
+ // Add interaction pair for pbd picking
+ imstkNew<PbdObjectPickingPair> upperJawPickingPair(clothObj, objUpperJaw, CollisionDetection::Type::PointSetToCapsule);
+ //imstkNew<PbdObjectPickingPair> lowerJawPickingPair(clothObj, objLowerJaw, CollisionDetection::Type::PointSetToCapsule);
+ scene->getCollisionGraph()->addInteraction(upperJawPickingPair);
+ //scene->getCollisionGraph()->addInteraction(lowerJawPickingPair);
+
+ // Camera
+ scene->getActiveCamera()->setPosition(Vec3d(1, 1, 1) * 100.0);
+ scene->getActiveCamera()->setFocalPoint(Vec3d(0, -50, 0));
+
+ // Light
+ imstkNew<DirectionalLight> light("light");
+ light->setFocalPoint(Vec3d(5.0, -8.0, -5.0));
+ light->setIntensity(1.0);
+ scene->addLight(light);
+
+ //Run the simulation
+ {
+ // Setup a viewer to render in its own thread
+ imstkNew<VTKViewer> viewer("Viewer");
+ viewer->setActiveScene(scene);
+
+ // Setup a scene manager to advance the scene in its own thread
+ imstkNew<SceneManager> sceneManager("Scene Manager");
+ sceneManager->setActiveScene(scene);
+ viewer->addChildThread(sceneManager); // SceneManager will start/stop with viewer
+
+ viewer->addChildThread(server);
+
+ // Add mouse and keyboard controls to the viewer
+ {
+ imstkNew<MouseSceneControl> mouseControl(viewer->getMouseDevice());
+ mouseControl->setSceneManager(sceneManager);
+ viewer->addControl(mouseControl);
+
+ imstkNew<KeyboardSceneControl> keyControl(viewer->getKeyboardDevice());
+ keyControl->setSceneManager(sceneManager);
+ keyControl->setViewer(viewer);
+ viewer->addControl(keyControl);
+ }
+ // Not perfectly thread safe movement lambda, ijkl movement instead of wasd because d is already used
+ std::shared_ptr<KeyboardDeviceClient> keyDevice = viewer->getKeyboardDevice();
+ connect<Event>(sceneManager, EventType::PreUpdate, [&](Event*)
+ {
+ std::shared_ptr<PBDPickingCH> chUpper = std::static_pointer_cast<PBDPickingCH>(upperJawPickingPair->getCollisionHandlingA());
+ //std::shared_ptr<PBDPickingCH> chLower = std::static_pointer_cast<PBDPickingCH>(lowerJawPickingPair->getCollisionHandlingA());
+
+ // Activate picking
+ if (client->getButton(1))
+ {
+ chUpper->activatePickConstraints();
+ //chLower->activatePickConstraints();
+ }
+ // Unpick
+ if (client->getButton(0))
+ {
+ chUpper->removePickConstraints();
+ //chLower->removePickConstraints();
+ }
+ });
+
+ // Start viewer running, scene as paused
+ sceneManager->requestStatus(ThreadStatus::Paused);
+ viewer->start();
+ }
+
+ return 0;
+}
diff --git a/Source/Controllers/imstkLaparoscopicToolController.cpp b/Source/Controllers/imstkLaparoscopicToolController.cpp
index c9991925f8ba3bf4eb8211837950e1808c3adcfa..6f3702b955eb62e74ab4a199ca23f6afcce9517e 100644
--- a/Source/Controllers/imstkLaparoscopicToolController.cpp
+++ b/Source/Controllers/imstkLaparoscopicToolController.cpp
@@ -1,134 +1,131 @@
-/*=========================================================================
-
- 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 "imstkDeviceClient.h"
-#include "imstkGeometry.h"
-#include "imstkLogger.h"
-
-namespace imstk
-{
-LaparoscopicToolController::LaparoscopicToolController(
- std::shared_ptr<SceneObject> shaft,
- std::shared_ptr<SceneObject> upperJaw,
- std::shared_ptr<SceneObject> lowerJaw,
- std::shared_ptr<DeviceClient> trackingDevice) :
- TrackingDeviceControl(trackingDevice),
- m_shaft(shaft),
- m_upperJaw(upperJaw),
- m_lowerJaw(lowerJaw),
- m_jawRotationAxis(Vec3d(1, 0, 0))
-{
- trackingDevice->setButtonsEnabled(true);
-
- // Record the transforms as 4x4 matrices (this should capture initial displacement/rotation of the jaws from the shaft)
- m_shaftWorldTransform = mat4dRotation(m_shaft->getMasterGeometry()->getRotation()) * mat4dTranslate(m_shaft->getMasterGeometry()->getTranslation());
-
- m_upperJawLocalTransform = mat4dRotation(m_upperJaw->getMasterGeometry()->getRotation()) * mat4dTranslate(m_upperJaw->getMasterGeometry()->getTranslation());
- m_lowerJawLocalTransform = mat4dRotation(m_lowerJaw->getMasterGeometry()->getRotation()) * mat4dTranslate(m_lowerJaw->getMasterGeometry()->getTranslation());
-
- m_upperJawWorldTransform = m_shaftWorldTransform * m_upperJawLocalTransform;
- m_lowerJawWorldTransform = m_shaftWorldTransform * m_lowerJawLocalTransform;
-}
-
-void
-LaparoscopicToolController::updateControlledObjects()
-{
- if (!isTrackerUpToDate())
- {
- if (!updateTrackingData())
- {
- LOG(WARNING) << "LaparoscopicToolController::updateControlledObjects warning: could not update tracking info.";
- return;
- }
- }
-
- const Vec3d controllerPosition = getPosition();
- const Quatd controllerOrientation = getRotation();
-
- // Controller transform
- m_shaftWorldTransform = mat4dTranslate(controllerPosition) * mat4dRotation(controllerOrientation);
- {
- // TRS decompose and set shaft
- Vec3d t, s;
- Mat3d r;
- mat4dTRS(m_shaftWorldTransform, t, r, s);
- m_shaft->getMasterGeometry()->setRotation(r);
- m_shaft->getMasterGeometry()->setTranslation(t);
- }
-
- // Update jaw angles
- if (m_deviceClient->getButton(0))
- {
- m_jawAngle += m_change;
- m_jawAngle = (m_jawAngle > m_maxJawAngle) ? m_maxJawAngle : m_jawAngle;
- }
- if (m_deviceClient->getButton(1))
- {
- m_jawAngle -= m_change;
- m_jawAngle = (m_jawAngle < 0.0) ? 0.0 : m_jawAngle;
- }
-
- m_upperJawLocalTransform = mat4dRotation(Rotd(m_jawAngle, m_jawRotationAxis));
- m_lowerJawLocalTransform = mat4dRotation(Rotd(-m_jawAngle, m_jawRotationAxis));
- m_upperJawWorldTransform = m_shaftWorldTransform * m_upperJawLocalTransform;
- m_lowerJawWorldTransform = m_shaftWorldTransform * m_lowerJawLocalTransform;
-
- {
- Vec3d t, s;
- Mat3d r;
- mat4dTRS(m_upperJawWorldTransform, t, r, s);
- m_upperJaw->getMasterGeometry()->setRotation(r);
- m_upperJaw->getMasterGeometry()->setTranslation(t);
- }
- {
- Vec3d t, s;
- Mat3d r;
- mat4dTRS(m_lowerJawWorldTransform, t, r, s);
- m_lowerJaw->getMasterGeometry()->setRotation(r);
- m_lowerJaw->getMasterGeometry()->setTranslation(t);
- }
-}
-
-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_deviceClient->setForce(force);
-}
-} // imstk
+/*=========================================================================
+
+ 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 "imstkDeviceClient.h"
+#include "imstkGeometry.h"
+#include "imstkLogger.h"
+
+namespace imstk
+{
+LaparoscopicToolController::LaparoscopicToolController(
+ std::shared_ptr<SceneObject> shaft,
+ std::shared_ptr<SceneObject> upperJaw,
+ std::shared_ptr<SceneObject> lowerJaw,
+ std::shared_ptr<DeviceClient> trackingDevice) :
+ TrackingDeviceControl(trackingDevice),
+ m_shaft(shaft),
+ m_upperJaw(upperJaw),
+ m_lowerJaw(lowerJaw),
+ m_jawRotationAxis(Vec3d(1, 0, 0))
+{
+ trackingDevice->setButtonsEnabled(true);
+
+ // Record the transforms as 4x4 matrices (this should capture initial displacement/rotation of the jaws from the shaft)
+ m_shaftInitialTransform = mat4dTranslate(m_shaft->getMasterGeometry()->getTranslation()) * mat4dRotation(m_shaft->getMasterGeometry()->getRotation());
+ m_upperJawInitialTransform = mat4dTranslate(m_upperJaw->getMasterGeometry()->getTranslation()) * mat4dRotation(m_upperJaw->getMasterGeometry()->getRotation());
+ m_lowerJawInitialTransform = mat4dTranslate(m_lowerJaw->getMasterGeometry()->getTranslation()) * mat4dRotation(m_lowerJaw->getMasterGeometry()->getRotation());
+}
+
+void
+LaparoscopicToolController::updateControlledObjects()
+{
+ if (!isTrackerUpToDate())
+ {
+ if (!updateTrackingData())
+ {
+ LOG(WARNING) << "LaparoscopicToolController::updateControlledObjects warning: could not update tracking info.";
+ return;
+ }
+ }
+
+ const Vec3d controllerPosition = getPosition();
+ const Quatd controllerOrientation = getRotation();
+
+ // Controller transform
+ m_controllerWorldTransform = mat4dTranslate(controllerPosition) * mat4dRotation(controllerOrientation);
+ m_shaftWorldTransform = m_controllerWorldTransform * m_shaftInitialTransform;
+ {
+ // TRS decompose and set shaft
+ Vec3d t, s;
+ Mat3d r;
+ mat4dTRS(m_shaftWorldTransform, t, r, s);
+ m_shaft->getMasterGeometry()->setRotation(r);
+ m_shaft->getMasterGeometry()->setTranslation(t);
+ }
+
+ // Update jaw angles
+ if (m_deviceClient->getButton(0))
+ {
+ m_jawAngle += m_change;
+ m_jawAngle = (m_jawAngle > m_maxJawAngle) ? m_maxJawAngle : m_jawAngle;
+ }
+ if (m_deviceClient->getButton(1))
+ {
+ m_jawAngle -= m_change;
+ m_jawAngle = (m_jawAngle < 0.0) ? 0.0 : m_jawAngle;
+ }
+
+ m_upperJawLocalTransform = mat4dRotation(Rotd(m_jawAngle, m_jawRotationAxis));
+ m_lowerJawLocalTransform = mat4dRotation(Rotd(-m_jawAngle, m_jawRotationAxis));
+ m_upperJawWorldTransform = m_controllerWorldTransform * m_upperJawLocalTransform * m_upperJawInitialTransform;
+ m_lowerJawWorldTransform = m_controllerWorldTransform * m_lowerJawLocalTransform * m_upperJawInitialTransform;
+
+ {
+ Vec3d t, s;
+ Mat3d r;
+ mat4dTRS(m_upperJawWorldTransform, t, r, s);
+ m_upperJaw->getMasterGeometry()->setRotation(r);
+ m_upperJaw->getMasterGeometry()->setTranslation(t);
+ }
+ {
+ Vec3d t, s;
+ Mat3d r;
+ mat4dTRS(m_lowerJawWorldTransform, t, r, s);
+ m_lowerJaw->getMasterGeometry()->setRotation(r);
+ m_lowerJaw->getMasterGeometry()->setTranslation(t);
+ }
+}
+
+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_deviceClient->setForce(force);
+}
+} // imstk
diff --git a/Source/Controllers/imstkLaparoscopicToolController.h b/Source/Controllers/imstkLaparoscopicToolController.h
index 4b7d498fa154b02debd9165887771f9efd69047a..c29d034a30b7e449eb0432c9e0c086e9b91cb9cc 100644
--- a/Source/Controllers/imstkLaparoscopicToolController.h
+++ b/Source/Controllers/imstkLaparoscopicToolController.h
@@ -1,109 +1,114 @@
-/*=========================================================================
-
- 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.
-
-=========================================================================*/
-
-#pragma once
-
-#include "imstkTrackingDeviceControl.h"
-
-namespace imstk
-{
-class SceneObject;
-
-///
-/// \class LaparoscopicToolController
-///
-/// \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 TrackingDeviceControl
-{
-public:
- ///
- /// \brief Constructor
- ///
- LaparoscopicToolController(
- std::shared_ptr<SceneObject> shaft,
- std::shared_ptr<SceneObject> upperJaw,
- std::shared_ptr<SceneObject> lowerJaw,
- std::shared_ptr<DeviceClient> trackingDevice);
-
- LaparoscopicToolController() = delete; //not allowed for now
-
- ~LaparoscopicToolController() override = default;
-
-public:
- ///
- /// \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 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; }
-
-protected:
- 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; ///< Angle of the jaws
-
- Mat4d m_shaftWorldTransform = Mat4d::Identity(); // Final world transform of the shaft
-
- Mat4d m_upperJawWorldTransform = Mat4d::Identity(); // Final world transform of the upper jaw
- Mat4d m_lowerJawWorldTransform = Mat4d::Identity();// Final world transform of the lower jaw
-
- Mat4d m_upperJawLocalTransform = Mat4d::Identity(); // m_upperJawWorldTransform = m_shaftWorldTransform * m_upperJawLocalTransform
- Mat4d m_lowerJawLocalTransform = Mat4d::Identity(); // m_lowerJawWorldTransform = m_shaftWorldTransform * m_lowerJawLocalTransform
-};
-} // imstk
+/*=========================================================================
+
+ 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.
+
+=========================================================================*/
+
+#pragma once
+
+#include "imstkTrackingDeviceControl.h"
+
+namespace imstk
+{
+class SceneObject;
+
+///
+/// \class LaparoscopicToolController
+///
+/// \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 TrackingDeviceControl
+{
+public:
+ ///
+ /// \brief Constructor
+ ///
+ LaparoscopicToolController(
+ std::shared_ptr<SceneObject> shaft,
+ std::shared_ptr<SceneObject> upperJaw,
+ std::shared_ptr<SceneObject> lowerJaw,
+ std::shared_ptr<DeviceClient> trackingDevice);
+
+ LaparoscopicToolController() = delete; //not allowed for now
+
+ ~LaparoscopicToolController() override = default;
+
+public:
+ ///
+ /// \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 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; }
+
+protected:
+ 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; ///< Angle of the jaws
+
+ Mat4d m_controllerWorldTransform = Mat4d::Identity(); // Final world transform of the controller
+
+ Mat4d m_shaftWorldTransform = Mat4d::Identity(); // Final world transform of the shaft
+ Mat4d m_upperJawWorldTransform = Mat4d::Identity(); // Final world transform of the upper jaw
+ Mat4d m_lowerJawWorldTransform = Mat4d::Identity(); // Final world transform of the lower jaw
+
+ Mat4d m_shaftInitialTransform = Mat4d::Identity(); // Initial local transform of the shaft
+ Mat4d m_upperJawInitialTransform = Mat4d::Identity(); // Initial local transform of the upper jaw
+ Mat4d m_lowerJawInitialTransform = Mat4d::Identity(); // Initial local transform of the lower jaw
+
+ Mat4d m_upperJawLocalTransform = Mat4d::Identity(); // m_upperJawWorldTransform = m_controllerWorldTransform * m_upperJawLocalTransform * m_upperJawInitialTransform
+ Mat4d m_lowerJawLocalTransform = Mat4d::Identity(); // m_lowerJawWorldTransform = m_controllerWorldTransform * m_lowerJawLocalTransform * m_lowerJawInitialTransform
+};
+} // imstk