diff --git a/CMake/External/External_iMSTKData.cmake b/CMake/External/External_iMSTKData.cmake
index df40d3d47823bcd013044bddb9b781e43c2f4014..445a926f42082a86be6e102cbb8242c45988766e 100644
--- a/CMake/External/External_iMSTKData.cmake
+++ b/CMake/External/External_iMSTKData.cmake
@@ -16,7 +16,7 @@ set(copy_data_command
)
include(imstkAddExternalProject)
-set(GIT_SHA "14824e3d53328ed6be481981959780f32881030b")
+set(GIT_SHA "2166a3715358e9794e53baf5dfa7deeaf36321cd")
set(DATA_URL "https://gitlab.kitware.com/iMSTK/imstk-data/-/archive/${GIT_SHA}/imstk-data-${GIT_SHA}.zip")
imstk_add_external_project( iMSTKData
diff --git a/Examples/FemurCut/FemurCutExample.cpp b/Examples/FemurCut/FemurCutExample.cpp
index 8d5eb2225fc6393616473ffff85e962a9d5b6229..7cef4280863b824cbe8199924b0b1d7ab6bab814 100644
--- a/Examples/FemurCut/FemurCutExample.cpp
+++ b/Examples/FemurCut/FemurCutExample.cpp
@@ -97,7 +97,6 @@ main()
Logger::startLogger();
imstkNew<Scene> scene("FemurCut");
- scene->getConfig()->taskParallelizationEnabled = false;
imstkNew<FemurObject> femurObj;
scene->addSceneObject(femurObj);
diff --git a/Examples/PBDCutting/CMakeLists.txt b/Examples/PBD/PBDCutting/CMakeLists.txt
similarity index 100%
rename from Examples/PBDCutting/CMakeLists.txt
rename to Examples/PBD/PBDCutting/CMakeLists.txt
diff --git a/Examples/PBDCutting/PBDCuttingExample.cpp b/Examples/PBD/PBDCutting/PBDCuttingExample.cpp
similarity index 100%
rename from Examples/PBDCutting/PBDCuttingExample.cpp
rename to Examples/PBD/PBDCutting/PBDCuttingExample.cpp
diff --git a/Examples/PBDPicking/CMakeLists.txt b/Examples/PBD/PBDPicking/CMakeLists.txt
similarity index 100%
rename from Examples/PBDPicking/CMakeLists.txt
rename to Examples/PBD/PBDPicking/CMakeLists.txt
diff --git a/Examples/PBDPicking/PBDPickingExample.cpp b/Examples/PBD/PBDPicking/PBDPickingExample.cpp
similarity index 97%
rename from Examples/PBDPicking/PBDPickingExample.cpp
rename to Examples/PBD/PBDPicking/PBDPickingExample.cpp
index e61d8000153efc0ca5008f276d77fb77bb9efbbf..11d46fde1dcd7d5ccbb702f2440575e0b8d29412 100644
--- a/Examples/PBDPicking/PBDPickingExample.cpp
+++ b/Examples/PBD/PBDPicking/PBDPickingExample.cpp
@@ -1,297 +1,297 @@
-/*=========================================================================
-
- 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 "imstkCapsule.h"
-#include "imstkCollisionGraph.h"
-#include "imstkDirectionalLight.h"
-#include "imstkHapticDeviceClient.h"
-#include "imstkHapticDeviceManager.h"
-#include "imstkKeyboardSceneControl.h"
-#include "imstkLaparoscopicToolController.h"
-#include "imstkLogger.h"
-#include "imstkMeshIO.h"
-#include "imstkMouseSceneControl.h"
-#include "imstkNew.h"
-#include "imstkPbdModel.h"
-#include "imstkPbdObject.h"
-#include "imstkPbdObjectCollision.h"
-#include "imstkPbdObjectPicking.h"
-#include "imstkPBDPickingCH.h"
-#include "imstkRenderMaterial.h"
-#include "imstkScene.h"
-#include "imstkSceneManager.h"
-#include "imstkSimulationManager.h"
-#include "imstkSurfaceMesh.h"
-#include "imstkVisualModel.h"
-#include "imstkVTKViewer.h"
-
-using namespace imstk;
-
-///
-/// \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;
-
- imstkNew<VecDataArray<double, 3>> verticesPtr(nRows * nCols);
- VecDataArray<double, 3>& vertices = *verticesPtr.get();
- const double dy = width / static_cast<double>(nCols - 1);
- const double dx = height / static_cast<double>(nRows - 1);
- for (int i = 0; i < nRows; i++)
- {
- for (int j = 0; j < nCols; j++)
- {
- vertices[i * nCols + j] = Vec3d(dx * static_cast<double>(i), 1.0, dy * static_cast<double>(j));
- }
- }
-
- // Add connectivity data
- imstkNew<VecDataArray<int, 3>> indicesPtr;
- VecDataArray<int, 3>& indices = *indicesPtr.get();
- for (int i = 0; i < nRows - 1; i++)
- {
- for (int j = 0; j < nCols - 1; j++)
- {
- const int index1 = i * nCols + j;
- const int index2 = index1 + nCols;
- const int index3 = index1 + 1;
- const int index4 = index2 + 1;
-
- // Interleave [/][\] pattern
- if (i % 2 ^ j % 2)
- {
- indices.push_back(Vec3i(index1, index2, index3));
- indices.push_back(Vec3i(index4, index3, index2));
- }
- else
- {
- indices.push_back(Vec3i(index2, index4, index1));
- indices.push_back(Vec3i(index4, index3, index1));
- }
- }
- }
-
- clothMesh->initialize(verticesPtr, indicesPtr);
-
- 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, 4000.0);
- pbdParams->enableConstraint(PbdConstraint::Type::Dihedral, 100.0);
- 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, -140.0, 0.0);
- pbdParams->m_dt = 0.01;
- pbdParams->m_iterations = 5;
- pbdParams->m_viscousDampingCoeff = 0.01;
-
- // 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;
- server->setSleepDelay(1.0);
- std::shared_ptr<HapticDeviceClient> client = server->makeDeviceClient();
-
- // 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->setOrientation(Quatd(Rotd(PI_2, Vec3d(1.0, 0.0, 0.0))));
- geomShaft->setTranslation(Vec3d(0.0, 0.0, 10.0));
- imstkNew<CollidingObject> objShaft("ShaftObject");
- objShaft->setVisualGeometry(pivotSurfMesh);
- objShaft->setCollidingGeometry(geomShaft);
- scene->addSceneObject(objShaft);
-
- imstkNew<Capsule> geomUpperJaw;
- geomUpperJaw->setLength(25.0);
- geomUpperJaw->setTranslation(Vec3d(0.0, 1.0, -12.5));
- geomUpperJaw->setRadius(2.0);
- geomUpperJaw->setOrientation(Quatd(Rotd(PI_2, Vec3d(1.0, 0.0, 0.0))));
- imstkNew<CollidingObject> objUpperJaw("UpperJawObject");
- objUpperJaw->setVisualGeometry(upperSurfMesh);
- objUpperJaw->setCollidingGeometry(geomUpperJaw);
- scene->addSceneObject(objUpperJaw);
-
- imstkNew<Capsule> geomLowerJaw;
- geomLowerJaw->setLength(25.0);
- geomLowerJaw->setTranslation(Vec3d(0.0, -1.0, -12.5));
- geomLowerJaw->setRadius(2.0);
- geomLowerJaw->setOrientation(Quatd(Rotd(PI_2, Vec3d(1.0, 0.0, 0.0))));
- imstkNew<CollidingObject> objLowerJaw("LowerJawObject");
- objLowerJaw->setVisualGeometry(lowerSurfMesh);
- objLowerJaw->setCollidingGeometry(geomLowerJaw);
- scene->addSceneObject(objLowerJaw);
-
- std::shared_ptr<PbdObject> clothObj = makeClothObj("Cloth", 50.0, 50.0, 15, 15);
- 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 collision for both jaws of the tool
- auto upperJawCollision = std::make_shared<PbdObjectCollision>(clothObj, objUpperJaw, "SurfaceMeshToCapsuleCD");
- auto lowerJawCollision = std::make_shared<PbdObjectCollision>(clothObj, objLowerJaw, "SurfaceMeshToCapsuleCD");
- scene->getCollisionGraph()->addInteraction(upperJawCollision);
- scene->getCollisionGraph()->addInteraction(lowerJawCollision);
-
- // Add picking interaction for both jaws of the tool
- auto upperJawPicking = std::make_shared<PbdObjectPicking>(clothObj, objUpperJaw, "PointSetToCapsuleCD");
- auto lowerJawPicking = std::make_shared<PbdObjectPicking>(clothObj, objLowerJaw, "PointSetToCapsuleCD");
- scene->getCollisionGraph()->addInteraction(upperJawPicking);
- scene->getCollisionGraph()->addInteraction(lowerJawPicking);
-
- // Camera
- scene->getActiveCamera()->setPosition(Vec3d(1.0, 1.0, 1.0) * 100.0);
- scene->getActiveCamera()->setFocalPoint(Vec3d(0.0, -50.0, 0.0));
-
- // Light
- imstkNew<DirectionalLight> light;
- light->setFocalPoint(Vec3d(5.0, -8.0, -5.0));
- light->setIntensity(1.0);
- scene->addLight("light", light);
-
- // Run the simulation
- {
- // Setup a viewer to render
- imstkNew<VTKViewer> viewer("Viewer");
- viewer->setActiveScene(scene);
-
- // Setup a scene manager to advance the scene
- imstkNew<SceneManager> sceneManager("Scene Manager");
- sceneManager->setActiveScene(scene);
- sceneManager->setExecutionType(Module::ExecutionType::ADAPTIVE);
- sceneManager->pause(); // Start simulation paused
-
- imstkNew<SimulationManager> driver;
- driver->addModule(server);
- driver->addModule(viewer);
- driver->addModule(sceneManager);
- driver->setDesiredDt(0.005);
-
- // 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->setModuleDriver(driver);
- viewer->addControl(keyControl);
- }
-
- connect<Event>(sceneManager, &SceneManager::postUpdate, [&](Event*)
- {
- // Simulate the cloth in real time
- clothObj->getPbdModel()->getParameters()->m_dt = sceneManager->getDt();
- });
-
- connect<Event>(controller, &LaparoscopicToolController::JawClosed,
- [&](Event*)
- {
- LOG(INFO) << "Jaw Closed!";
- auto chUpper = std::dynamic_pointer_cast<PBDPickingCH>(upperJawPicking->getCollisionHandlingA());
- auto chLower = std::dynamic_pointer_cast<PBDPickingCH>(lowerJawPicking->getCollisionHandlingA());
-
- chUpper->beginPick();
- chLower->beginPick();
- });
- connect<Event>(controller, &LaparoscopicToolController::JawOpened,
- [&](Event*)
- {
- LOG(INFO) << "Jaw Opened!";
- auto chUpper = std::dynamic_pointer_cast<PBDPickingCH>(upperJawPicking->getCollisionHandlingA());
- auto chLower = std::dynamic_pointer_cast<PBDPickingCH>(lowerJawPicking->getCollisionHandlingA());
-
- chUpper->endPick();
- chLower->endPick();
- });
-
- driver->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 "imstkCapsule.h"
+#include "imstkCollisionGraph.h"
+#include "imstkDirectionalLight.h"
+#include "imstkHapticDeviceClient.h"
+#include "imstkHapticDeviceManager.h"
+#include "imstkKeyboardSceneControl.h"
+#include "imstkLaparoscopicToolController.h"
+#include "imstkLogger.h"
+#include "imstkMeshIO.h"
+#include "imstkMouseSceneControl.h"
+#include "imstkNew.h"
+#include "imstkPbdModel.h"
+#include "imstkPbdObject.h"
+#include "imstkPbdObjectCollision.h"
+#include "imstkPbdObjectPicking.h"
+#include "imstkPBDPickingCH.h"
+#include "imstkRenderMaterial.h"
+#include "imstkScene.h"
+#include "imstkSceneManager.h"
+#include "imstkSimulationManager.h"
+#include "imstkSurfaceMesh.h"
+#include "imstkVisualModel.h"
+#include "imstkVTKViewer.h"
+
+using namespace imstk;
+
+///
+/// \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;
+
+ imstkNew<VecDataArray<double, 3>> verticesPtr(nRows * nCols);
+ VecDataArray<double, 3>& vertices = *verticesPtr.get();
+ const double dy = width / static_cast<double>(nCols - 1);
+ const double dx = height / static_cast<double>(nRows - 1);
+ for (int i = 0; i < nRows; i++)
+ {
+ for (int j = 0; j < nCols; j++)
+ {
+ vertices[i * nCols + j] = Vec3d(dx * static_cast<double>(i), 1.0, dy * static_cast<double>(j));
+ }
+ }
+
+ // Add connectivity data
+ imstkNew<VecDataArray<int, 3>> indicesPtr;
+ VecDataArray<int, 3>& indices = *indicesPtr.get();
+ for (int i = 0; i < nRows - 1; i++)
+ {
+ for (int j = 0; j < nCols - 1; j++)
+ {
+ const int index1 = i * nCols + j;
+ const int index2 = index1 + nCols;
+ const int index3 = index1 + 1;
+ const int index4 = index2 + 1;
+
+ // Interleave [/][\] pattern
+ if (i % 2 ^ j % 2)
+ {
+ indices.push_back(Vec3i(index1, index2, index3));
+ indices.push_back(Vec3i(index4, index3, index2));
+ }
+ else
+ {
+ indices.push_back(Vec3i(index2, index4, index1));
+ indices.push_back(Vec3i(index4, index3, index1));
+ }
+ }
+ }
+
+ clothMesh->initialize(verticesPtr, indicesPtr);
+
+ 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, 4000.0);
+ pbdParams->enableConstraint(PbdConstraint::Type::Dihedral, 100.0);
+ 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, -140.0, 0.0);
+ pbdParams->m_dt = 0.01;
+ pbdParams->m_iterations = 5;
+ pbdParams->m_viscousDampingCoeff = 0.01;
+
+ // 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;
+ server->setSleepDelay(1.0);
+ std::shared_ptr<HapticDeviceClient> client = server->makeDeviceClient();
+
+ // 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->setOrientation(Quatd(Rotd(PI_2, Vec3d(1.0, 0.0, 0.0))));
+ geomShaft->setTranslation(Vec3d(0.0, 0.0, 10.0));
+ imstkNew<CollidingObject> objShaft("ShaftObject");
+ objShaft->setVisualGeometry(pivotSurfMesh);
+ objShaft->setCollidingGeometry(geomShaft);
+ scene->addSceneObject(objShaft);
+
+ imstkNew<Capsule> geomUpperJaw;
+ geomUpperJaw->setLength(25.0);
+ geomUpperJaw->setTranslation(Vec3d(0.0, 1.0, -12.5));
+ geomUpperJaw->setRadius(2.0);
+ geomUpperJaw->setOrientation(Quatd(Rotd(PI_2, Vec3d(1.0, 0.0, 0.0))));
+ imstkNew<CollidingObject> objUpperJaw("UpperJawObject");
+ objUpperJaw->setVisualGeometry(upperSurfMesh);
+ objUpperJaw->setCollidingGeometry(geomUpperJaw);
+ scene->addSceneObject(objUpperJaw);
+
+ imstkNew<Capsule> geomLowerJaw;
+ geomLowerJaw->setLength(25.0);
+ geomLowerJaw->setTranslation(Vec3d(0.0, -1.0, -12.5));
+ geomLowerJaw->setRadius(2.0);
+ geomLowerJaw->setOrientation(Quatd(Rotd(PI_2, Vec3d(1.0, 0.0, 0.0))));
+ imstkNew<CollidingObject> objLowerJaw("LowerJawObject");
+ objLowerJaw->setVisualGeometry(lowerSurfMesh);
+ objLowerJaw->setCollidingGeometry(geomLowerJaw);
+ scene->addSceneObject(objLowerJaw);
+
+ std::shared_ptr<PbdObject> clothObj = makeClothObj("Cloth", 50.0, 50.0, 15, 15);
+ 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 collision for both jaws of the tool
+ auto upperJawCollision = std::make_shared<PbdObjectCollision>(clothObj, objUpperJaw, "SurfaceMeshToCapsuleCD");
+ auto lowerJawCollision = std::make_shared<PbdObjectCollision>(clothObj, objLowerJaw, "SurfaceMeshToCapsuleCD");
+ scene->getCollisionGraph()->addInteraction(upperJawCollision);
+ scene->getCollisionGraph()->addInteraction(lowerJawCollision);
+
+ // Add picking interaction for both jaws of the tool
+ auto upperJawPicking = std::make_shared<PbdObjectPicking>(clothObj, objUpperJaw, "PointSetToCapsuleCD");
+ auto lowerJawPicking = std::make_shared<PbdObjectPicking>(clothObj, objLowerJaw, "PointSetToCapsuleCD");
+ scene->getCollisionGraph()->addInteraction(upperJawPicking);
+ scene->getCollisionGraph()->addInteraction(lowerJawPicking);
+
+ // Camera
+ scene->getActiveCamera()->setPosition(Vec3d(1.0, 1.0, 1.0) * 100.0);
+ scene->getActiveCamera()->setFocalPoint(Vec3d(0.0, -50.0, 0.0));
+
+ // Light
+ imstkNew<DirectionalLight> light;
+ light->setFocalPoint(Vec3d(5.0, -8.0, -5.0));
+ light->setIntensity(1.0);
+ scene->addLight("light", light);
+
+ // Run the simulation
+ {
+ // Setup a viewer to render
+ imstkNew<VTKViewer> viewer("Viewer");
+ viewer->setActiveScene(scene);
+
+ // Setup a scene manager to advance the scene
+ imstkNew<SceneManager> sceneManager("Scene Manager");
+ sceneManager->setActiveScene(scene);
+ sceneManager->setExecutionType(Module::ExecutionType::ADAPTIVE);
+ sceneManager->pause(); // Start simulation paused
+
+ imstkNew<SimulationManager> driver;
+ driver->addModule(server);
+ driver->addModule(viewer);
+ driver->addModule(sceneManager);
+ driver->setDesiredDt(0.005);
+
+ // 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->setModuleDriver(driver);
+ viewer->addControl(keyControl);
+ }
+
+ connect<Event>(sceneManager, &SceneManager::postUpdate, [&](Event*)
+ {
+ // Simulate the cloth in real time
+ clothObj->getPbdModel()->getParameters()->m_dt = sceneManager->getDt();
+ });
+
+ connect<Event>(controller, &LaparoscopicToolController::JawClosed,
+ [&](Event*)
+ {
+ LOG(INFO) << "Jaw Closed!";
+ auto chUpper = std::dynamic_pointer_cast<PBDPickingCH>(upperJawPicking->getCollisionHandlingA());
+ auto chLower = std::dynamic_pointer_cast<PBDPickingCH>(lowerJawPicking->getCollisionHandlingA());
+
+ chUpper->beginPick();
+ chLower->beginPick();
+ });
+ connect<Event>(controller, &LaparoscopicToolController::JawOpened,
+ [&](Event*)
+ {
+ LOG(INFO) << "Jaw Opened!";
+ auto chUpper = std::dynamic_pointer_cast<PBDPickingCH>(upperJawPicking->getCollisionHandlingA());
+ auto chLower = std::dynamic_pointer_cast<PBDPickingCH>(lowerJawPicking->getCollisionHandlingA());
+
+ chUpper->endPick();
+ chLower->endPick();
+ });
+
+ driver->start();
+ }
+
+ return 0;
+}
diff --git a/Examples/PBD/PBDStaticSuture/CMakeLists.txt b/Examples/PBD/PBDStaticSuture/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..c8ba865d36da86763bcbcd6b981b97b2d15447d0
--- /dev/null
+++ b/Examples/PBD/PBDStaticSuture/CMakeLists.txt
@@ -0,0 +1,44 @@
+###########################################################################
+#
+# Copyright (c) Kitware, Inc.
+#
+# 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.
+#
+###########################################################################
+
+project(Example-PBDStaticSuture)
+
+if (iMSTK_USE_OpenHaptics)
+ #-----------------------------------------------------------------------------
+ # Create executable
+ #-----------------------------------------------------------------------------
+ imstk_add_executable(${PROJECT_NAME}
+ pbdStaticSutureExample.cpp
+ NeedleInteraction.h
+ NeedleInteraction.cpp
+ NeedleObject.h
+ NeedleObject.cpp
+ NeedleRigidBodyCH.h
+ NeedleRigidBodyCH.cpp
+ RbdPointToArcConstraint.h)
+
+ #-----------------------------------------------------------------------------
+ # Add the target to Examples folder
+ #-----------------------------------------------------------------------------
+ SET_TARGET_PROPERTIES (${PROJECT_NAME} PROPERTIES FOLDER Examples/PBD)
+
+ #-----------------------------------------------------------------------------
+ # Link libraries to executable
+ #-----------------------------------------------------------------------------
+ target_link_libraries(${PROJECT_NAME} SimulationManager)
+endif()
diff --git a/Examples/PBD/PBDStaticSuture/NeedleInteraction.cpp b/Examples/PBD/PBDStaticSuture/NeedleInteraction.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..004832f9dc6c8f749ee484435e4d06378f97206d
--- /dev/null
+++ b/Examples/PBD/PBDStaticSuture/NeedleInteraction.cpp
@@ -0,0 +1,55 @@
+/*=========================================================================
+
+ 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 "NeedleInteraction.h"
+#include "NeedleObject.h"
+#include "NeedleRigidBodyCH.h"
+#include "imstkCollisionDetectionAlgorithm.h"
+#include "imstkLineMesh.h"
+#include "imstkImplicitGeometry.h"
+#include "imstkTaskGraph.h"
+
+using namespace imstk;
+
+NeedleInteraction::NeedleInteraction(std::shared_ptr<CollidingObject> tissueObj,
+ std::shared_ptr<NeedleObject> needleObj) : RigidObjectCollision(needleObj, tissueObj, "ImplicitGeometryToPointSetCD")
+{
+ if (std::dynamic_pointer_cast<LineMesh>(needleObj->getCollidingGeometry()) == nullptr)
+ {
+ LOG(WARNING) << "NeedleInteraction only works with LineMesh collision geometry on rigid DrillObject";
+ }
+ if (std::dynamic_pointer_cast<ImplicitGeometry>(tissueObj->getCollidingGeometry()) == nullptr)
+ {
+ LOG(WARNING) << "NeedleInteraction only works with SDF colliding geometry on colliding tissueObj";
+ }
+
+ // First replace the handlers with our needle subclasses
+
+ // This handler consumes collision data to resolve the tool from the tissue
+ // except when the needle is inserted
+ auto needleRbdCH = std::make_shared<NeedleRigidBodyCH>();
+ needleRbdCH->setInputRigidObjectA(needleObj);
+ needleRbdCH->setInputCollidingObjectB(tissueObj);
+ needleRbdCH->setInputCollisionData(getCollisionDetection()->getCollisionData());
+ needleRbdCH->setBeta(0.01);
+ needleRbdCH->getTaskNode()->m_isCritical = true;
+ setCollisionHandlingA(needleRbdCH);
+}
\ No newline at end of file
diff --git a/Examples/PBD/PBDStaticSuture/NeedleInteraction.h b/Examples/PBD/PBDStaticSuture/NeedleInteraction.h
new file mode 100644
index 0000000000000000000000000000000000000000..8d101daffa719a8fc2916746addbbfa4cbe87540
--- /dev/null
+++ b/Examples/PBD/PBDStaticSuture/NeedleInteraction.h
@@ -0,0 +1,41 @@
+/*=========================================================================
+
+ 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 "imstkRigidObjectCollision.h"
+
+using namespace imstk;
+
+class NeedleObject;
+
+///
+/// \class NeedleInteraction
+///
+/// \brief Defines interaction between NeedleObject and PbdObject
+///
+class NeedleInteraction : public RigidObjectCollision
+{
+public:
+ NeedleInteraction(std::shared_ptr<CollidingObject> tissueObj,
+ std::shared_ptr<NeedleObject> needleObj);
+ ~NeedleInteraction() override = default;
+};
\ No newline at end of file
diff --git a/Examples/PBD/PBDStaticSuture/NeedleObject.cpp b/Examples/PBD/PBDStaticSuture/NeedleObject.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d3ad917aae59aa30f13d0c21d107f03a3aef8fe9
--- /dev/null
+++ b/Examples/PBD/PBDStaticSuture/NeedleObject.cpp
@@ -0,0 +1,85 @@
+/*=========================================================================
+
+ 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 "NeedleObject.h"
+#include "imstkIsometricMap.h"
+#include "imstkLineMesh.h"
+#include "imstkMeshIO.h"
+#include "imstkRbdConstraint.h"
+#include "imstkRenderMaterial.h"
+#include "imstkRigidBodyModel2.h"
+#include "imstkSurfaceMesh.h"
+#include "imstkVecDataArray.h"
+#include "imstkVisualModel.h"
+
+using namespace imstk;
+
+NeedleObject::NeedleObject() : RigidObject2("Needle")
+{
+ auto sutureMesh = MeshIO::read<SurfaceMesh>(iMSTK_DATA_ROOT "/Surgical Instruments/Needles/c6_suture.stl");
+ auto sutureLineMesh = MeshIO::read<LineMesh>(iMSTK_DATA_ROOT "/Surgical Instruments/Needles/c6_suture_hull.vtk");
+
+ const Mat4d rot = mat4dRotation(Rotd(-PI_2, Vec3d(0.0, 1.0, 0.0))) *
+ mat4dRotation(Rotd(-0.6, Vec3d(1.0, 0.0, 0.0)));
+ sutureMesh->transform(rot, Geometry::TransformType::ApplyToData);
+ sutureLineMesh->transform(rot, Geometry::TransformType::ApplyToData);
+
+ setVisualGeometry(sutureMesh);
+ setCollidingGeometry(sutureLineMesh);
+ setPhysicsGeometry(sutureLineMesh);
+ setPhysicsToVisualMap(std::make_shared<IsometricMap>(sutureLineMesh, sutureMesh));
+ getVisualModel(0)->getRenderMaterial()->setColor(Color(0.9, 0.9, 0.9));
+ getVisualModel(0)->getRenderMaterial()->setShadingModel(RenderMaterial::ShadingModel::PBR);
+ getVisualModel(0)->getRenderMaterial()->setRoughness(0.5);
+ getVisualModel(0)->getRenderMaterial()->setMetalness(1.0);
+
+ std::shared_ptr<RigidBodyModel2> rbdModel = std::make_shared<RigidBodyModel2>();
+ rbdModel->getConfig()->m_gravity = Vec3d::Zero();
+ rbdModel->getConfig()->m_maxNumIterations = 5;
+ setDynamicalModel(rbdModel);
+
+ getRigidBody()->m_mass = 1.0;
+ getRigidBody()->m_intertiaTensor = Mat3d::Identity() * 10000.0;
+ getRigidBody()->m_initPos = Vec3d(0.0, 0.0, 0.0);
+
+ // Manually setup an arc aligned with the geometry, some sort of needle+arc generator
+ // could be a nice addition to imstk
+ Mat3d arcBasis = Mat3d::Identity();
+ arcBasis.col(0) = Vec3d(0.0, 0.0, -1.0);
+ arcBasis.col(1) = Vec3d(1.0, 0.0, 0.0);
+ arcBasis.col(2) = Vec3d(0.0, 1.0, 0.0);
+ arcBasis = rot.block<3, 3>(0, 0) * arcBasis;
+ const Vec3d arcCenter = (rot * Vec4d(0.0, -0.005455, 0.008839, 1.0)).head<3>();
+ const double arcRadius = 0.010705;
+ setArc(arcCenter, arcBasis, arcRadius, 0.558, 2.583);
+}
+
+const Mat3d
+NeedleObject::getArcBasis()
+{
+ return getRigidBody()->getOrientation().toRotationMatrix() * m_arcBasis;
+}
+
+const Vec3d
+NeedleObject::getArcCenter()
+{
+ return getRigidBody()->getPosition() + getRigidBody()->getOrientation().toRotationMatrix() * m_arcCenter;
+}
\ No newline at end of file
diff --git a/Examples/PBD/PBDStaticSuture/NeedleObject.h b/Examples/PBD/PBDStaticSuture/NeedleObject.h
new file mode 100644
index 0000000000000000000000000000000000000000..eb2698b85f909819b93554db6cbdf52d4019c704
--- /dev/null
+++ b/Examples/PBD/PBDStaticSuture/NeedleObject.h
@@ -0,0 +1,105 @@
+/*=========================================================================
+
+ 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 "imstkRigidObject2.h"
+
+using namespace imstk;
+
+class NeedleObject : public RigidObject2
+{
+public:
+ enum class CollisionState
+ {
+ REMOVED,
+ TOUCHING,
+ INSERTED
+ };
+
+public:
+ NeedleObject();
+ virtual ~NeedleObject() = default;
+
+ virtual const std::string getTypeName() const override { return "NeedleObject"; }
+
+public:
+ // *INDENT-OFF*
+ SIGNAL(NeedleObject, inserted);
+ SIGNAL(NeedleObject, removed);
+ // *INDENT-ON*
+
+public:
+ void setCollisionState(const CollisionState state)
+ {
+ // If current state is inserted and previous was not inserted
+ if (m_collisionState == CollisionState::INSERTED && state != CollisionState::INSERTED)
+ {
+ this->postEvent(Event(removed()));
+ }
+ // If current state not inserted and previous inserted
+ else if (m_collisionState != CollisionState::INSERTED && state == CollisionState::INSERTED)
+ {
+ this->postEvent(Event(inserted()));
+ }
+ m_collisionState = state;
+ }
+
+ CollisionState getCollisionState() const { return m_collisionState; }
+
+ ///
+ /// \brief Set the force threshold for the needle
+ ///
+ void setForceThreshold(const double forceThreshold) { m_forceThreshold = forceThreshold; }
+ double getForceThreshold() const { return m_forceThreshold; }
+
+ void setArc(const Vec3d& arcCenter, const Mat3d& arcBasis,
+ double arcRadius, double beginRad, double endRad)
+ {
+ m_arcCenter = arcCenter;
+ m_arcBasis = arcBasis;
+ m_beginRad = beginRad;
+ m_endRad = endRad;
+ m_arcRadius = arcRadius;
+ }
+
+ ///
+ /// \brief Get the basis post transformation of the rigid body
+ ///
+ const Mat3d getArcBasis();
+ ///
+ /// \brief Get the arc center post transformation of the rigid body
+ ///
+ const Vec3d getArcCenter();
+ const double getBeginRad() const { return m_beginRad; }
+ const double getEndRad() const { return m_endRad; }
+ const double getArcRadius() const { return m_arcRadius; }
+
+protected:
+ CollisionState m_collisionState = CollisionState::REMOVED;
+ double m_forceThreshold = 5.0;
+
+ Mat3d m_arcBasis = Mat3d::Identity();
+ Vec3d m_arcCenter = Vec3d::Zero();
+ double m_arcRadius = 1.0;
+ double m_beginRad = 0.0;
+ double m_endRad = PI * 2.0;
+};
\ No newline at end of file
diff --git a/Examples/PBD/PBDStaticSuture/NeedleRigidBodyCH.cpp b/Examples/PBD/PBDStaticSuture/NeedleRigidBodyCH.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4d08b626d270775a8c08f71b38ee8285c5810968
--- /dev/null
+++ b/Examples/PBD/PBDStaticSuture/NeedleRigidBodyCH.cpp
@@ -0,0 +1,121 @@
+/*=========================================================================
+
+ 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 "NeedleRigidBodyCH.h"
+#include "NeedleObject.h"
+#include "imstkRbdContactConstraint.h"
+#include "imstkRigidBodyModel2.h"
+#include "RbdPointToArcConstraint.h"
+
+void
+NeedleRigidBodyCH::handle(
+ const std::vector<CollisionElement>& elementsA,
+ const std::vector<CollisionElement>& elementsB)
+{
+ // Do it the normal way
+ RigidBodyCH::handle(elementsA, elementsB);
+
+ // If no collision, needle must be removed
+ auto needleObj = std::dynamic_pointer_cast<NeedleObject>(getInputObjectA());
+ NeedleObject::CollisionState state = needleObj->getCollisionState();
+
+ if (elementsA.size() != 0)
+ {
+ if (state == NeedleObject::CollisionState::INSERTED)
+ {
+ const Mat3d& arcBasis = needleObj->getArcBasis();
+ const Vec3d& arcCenter = needleObj->getArcCenter();
+ const double arcRadius = needleObj->getArcRadius();
+ const double arcBeginRad = needleObj->getBeginRad();
+ const double arcEndRad = needleObj->getEndRad();
+
+ // Constrain along the axes, whilst allowing "pushing" of the contact point
+ auto pointToArcConstraint = std::make_shared<RbdPointToArcConstraint>(
+ needleObj->getRigidBody(),
+ arcCenter,
+ arcBeginRad, arcEndRad,
+ arcRadius,
+ arcBasis,
+ m_initContactPt,
+ m_beta);
+ pointToArcConstraint->compute(needleObj->getRigidBodyModel2()->getTimeStep());
+ needleObj->getRigidBodyModel2()->addConstraint(pointToArcConstraint);
+ }
+ }
+ else
+ {
+ if (state == NeedleObject::CollisionState::INSERTED || state == NeedleObject::CollisionState::TOUCHING)
+ {
+ if (state == NeedleObject::CollisionState::INSERTED)
+ {
+ LOG(INFO) << "Unpuncture!";
+ }
+ needleObj->setCollisionState(NeedleObject::CollisionState::REMOVED);
+ }
+ }
+}
+
+void
+NeedleRigidBodyCH::addConstraint(
+ std::shared_ptr<RigidObject2> rbdObj,
+ const Vec3d& contactPt, const Vec3d& contactNormal,
+ const double contactDepth)
+{
+ auto obj = std::dynamic_pointer_cast<NeedleObject>(rbdObj);
+
+ // If removed and we are here, we must now be touching
+ if (obj->getCollisionState() == NeedleObject::CollisionState::REMOVED)
+ {
+ obj->setCollisionState(NeedleObject::CollisionState::TOUCHING);
+ }
+
+ // If touching we may test for insertion
+ const Vec3d n = contactNormal.normalized();
+ if (obj->getCollisionState() == NeedleObject::CollisionState::TOUCHING)
+ {
+ // Get all inwards force
+ const double fN = std::max(-contactNormal.dot(obj->getRigidBody()->getForce()), 0.0);
+
+ // If the normal force exceeds threshold the needle may insert
+ if (fN > obj->getForceThreshold())
+ {
+ LOG(INFO) << "Puncture!";
+ obj->setCollisionState(NeedleObject::CollisionState::INSERTED);
+
+ // Record the initial contact point
+ m_initOrientation = Quatd(rbdObj->getCollidingGeometry()->getRotation());
+ m_initContactPt = contactPt;
+ }
+ }
+
+ // Only add contact normal constraint if not inserted
+ NeedleObject::CollisionState state = obj->getCollisionState();
+ if (state == NeedleObject::CollisionState::TOUCHING)
+ {
+ auto contactConstraint = std::make_shared<RbdContactConstraint>(
+ rbdObj->getRigidBody(), nullptr,
+ n, contactPt, contactDepth,
+ m_beta,
+ RbdConstraint::Side::A);
+ contactConstraint->compute(rbdObj->getRigidBodyModel2()->getTimeStep());
+ rbdObj->getRigidBodyModel2()->addConstraint(contactConstraint);
+ }
+}
\ No newline at end of file
diff --git a/Examples/PBD/PBDStaticSuture/NeedleRigidBodyCH.h b/Examples/PBD/PBDStaticSuture/NeedleRigidBodyCH.h
new file mode 100644
index 0000000000000000000000000000000000000000..0e86780089a30d1fb2a078a43bce19c5a6b8c4de
--- /dev/null
+++ b/Examples/PBD/PBDStaticSuture/NeedleRigidBodyCH.h
@@ -0,0 +1,56 @@
+/*=========================================================================
+
+ 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 "imstkRigidBodyCH.h"
+
+using namespace imstk;
+
+class NeedleRigidBodyCH : public RigidBodyCH
+{
+public:
+ NeedleRigidBodyCH() = default;
+ ~NeedleRigidBodyCH() override = default;
+
+ virtual const std::string getTypeName() const override { return "NeedleRigidBodyCH"; }
+
+protected:
+ ///
+ /// \brief Handle the collision/contact data
+ ///
+ virtual void handle(
+ const std::vector<CollisionElement>& elementsA,
+ const std::vector<CollisionElement>& elementsB) override;
+
+ ///
+ /// \brief Add constraint for the rigid body given contact
+ ///
+ void addConstraint(
+ std::shared_ptr<RigidObject2> rbdObj,
+ const Vec3d& contactPt, const Vec3d& contactNormal,
+ const double contactDepth) override;
+
+protected:
+ Vec3d m_initContactPt = Vec3d::Zero();
+ Vec3d m_initAxes = Vec3d::Zero();
+ Quatd m_initOrientation = Quatd::Identity();
+};
\ No newline at end of file
diff --git a/Examples/PBD/PBDStaticSuture/RbdPointToArcConstraint.h b/Examples/PBD/PBDStaticSuture/RbdPointToArcConstraint.h
new file mode 100644
index 0000000000000000000000000000000000000000..78bbfe962180a183aa40695d59afffdd6d67311f
--- /dev/null
+++ b/Examples/PBD/PBDStaticSuture/RbdPointToArcConstraint.h
@@ -0,0 +1,108 @@
+/*=========================================================================
+
+ 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 "imstkRbdConstraint.h"
+
+using namespace imstk;
+
+///
+/// \class RbdPointToArcConstraint
+///
+/// \brief Constrains an rigid body arc geometry to a point by computing the
+/// linear force and angular torque to get the arc to the point
+///
+class RbdPointToArcConstraint : public RbdConstraint
+{
+public:
+ ///
+ /// \param the Rigid body
+ /// \param the center of the circle arc is defined with
+ /// \param the radians/range of the arc, with relation to the arcBasis
+ /// \param the radians/range of the arc, with relation to the arcBasis
+ /// \param the radius of the circle the arc is defined with
+ /// \param the basis of the arc. Where any point on the plane has a radian with relation to x,y columns.
+ /// and the z column gives the normal of the plane the circle+arc lie on
+ /// \param the fixed point
+ /// \param baumgarte stabilization, varies step length
+ ///
+ RbdPointToArcConstraint(
+ std::shared_ptr<RigidBody> obj,
+ const Vec3d arcCenter, const double beginRadian, const double endRadian,
+ const double arcCircleRadius, const Mat3d arcBasis,
+ const Vec3d fixedPoint,
+ const double beta = 0.05) : RbdConstraint(obj, nullptr, Side::A),
+ m_arcCenter(arcCenter), m_beginRadian(beginRadian), m_endRadian(endRadian),
+ m_arcCircleRadius(arcCircleRadius), m_arcBasis(arcBasis),
+ m_fixedPoint(fixedPoint),
+ m_beta(beta)
+ {
+ // Check orthonormal basis
+ }
+
+ ~RbdPointToArcConstraint() override = default;
+
+public:
+ void compute(double dt) override
+ {
+ // Jacobian of contact (defines linear and angular constraint axes)
+ J = Eigen::Matrix<double, 3, 4>::Zero();
+ if ((m_side == Side::AB || m_side == Side::A) && !m_obj1->m_isStatic)
+ {
+ // Compute the direction and closest point to the arc from the fixedPoint
+ const Vec3d circleDiff = m_fixedPoint - m_arcCenter;
+ const Vec3d dir = circleDiff.normalized();
+
+ // m_arcBasis Should be orthonormal, this should project onto the axes
+ const Mat3d invArcBasis = m_arcBasis.transpose();
+ const Vec3d p = invArcBasis * circleDiff;
+ const double rad = atan2(-p[2], -p[0]) + PI;
+ // Clamp to range (if closest point on circle is outside on range we want the end)
+ const double clampedRad = std::min(std::max(rad, m_beginRadian), m_endRadian);
+ // Finally compute the closest point to the arc using the new radian
+ const Vec3d closestPt = (cos(clampedRad) * m_arcBasis.col(0) +
+ sin(clampedRad) * m_arcBasis.col(2)) * m_arcCircleRadius +
+ m_arcCenter;
+
+ Vec3d diff = m_fixedPoint - closestPt;
+ const Vec3d r = closestPt - m_obj1->getPosition();
+ const Vec3d c = r.cross(diff);
+
+ vu = diff.norm() * m_beta / dt;
+ diff = diff.normalized();
+ J(0, 0) = diff[0]; J(0, 1) = c[0];
+ J(1, 0) = diff[1]; J(1, 1) = c[1];
+ J(2, 0) = diff[2]; J(2, 1) = c[2];
+ }
+ }
+
+private:
+ Vec3d m_arcCenter = Vec3d::Zero();
+ Mat3d m_arcBasis = Mat3d::Zero(); // Should be orthonormal
+ double m_arcCircleRadius = 0.0;
+ double m_beginRadian = 0.0;
+ double m_endRadian = 0.0;
+
+ Vec3d m_fixedPoint = Vec3d::Zero();
+
+ double m_beta = 0.05;
+};
\ No newline at end of file
diff --git a/Examples/PBD/PBDStaticSuture/pbdStaticSutureExample.cpp b/Examples/PBD/PBDStaticSuture/pbdStaticSutureExample.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4205458d6ea541f7b9807607dfc5c7910092703f
--- /dev/null
+++ b/Examples/PBD/PBDStaticSuture/pbdStaticSutureExample.cpp
@@ -0,0 +1,301 @@
+/*=========================================================================
+
+ 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.
+
+=========================================================================*/
+
+// This example is haptic only
+#include "imstkCamera.h"
+#include "imstkCollisionGraph.h"
+#include "imstkKeyboardSceneControl.h"
+#include "imstkLineMesh.h"
+#include "imstkLogger.h"
+#include "imstkMeshIO.h"
+#include "imstkMouseSceneControl.h"
+#include "imstkNew.h"
+#include "imstkOrientedBox.h"
+#include "imstkPbdModel.h"
+#include "imstkPbdObject.h"
+#include "imstkPbdObjectCollision.h"
+#include "imstkRbdConstraint.h"
+#include "imstkRenderMaterial.h"
+#include "imstkScene.h"
+#include "imstkSceneManager.h"
+#include "imstkSimulationManager.h"
+#include "imstkSurfaceMesh.h"
+#include "imstkVisualModel.h"
+#include "imstkVTKViewer.h"
+#include "NeedleInteraction.h"
+#include "NeedleObject.h"
+#include "imstkHapticDeviceManager.h"
+#include "imstkHapticDeviceClient.h"
+#include "imstkRigidObjectController.h"
+
+using namespace imstk;
+
+///
+/// \brief Create pbd string geometry
+///
+static std::shared_ptr<LineMesh>
+makeStringGeometry(const Vec3d& pos, const Vec3d& dx, const int numVerts)
+{
+ // Create the geometry
+ imstkNew<LineMesh> stringGeometry;
+
+ imstkNew<VecDataArray<double, 3>> verticesPtr(numVerts);
+ VecDataArray<double, 3>& vertices = *verticesPtr.get();
+ for (int i = 0; i < numVerts; i++)
+ {
+ vertices[i] = pos + dx * i;
+ }
+
+ // Add connectivity data
+ imstkNew<VecDataArray<int, 2>> segmentsPtr;
+ VecDataArray<int, 2>& segments = *segmentsPtr.get();
+ for (int i = 0; i < numVerts - 1; i++)
+ {
+ segments.push_back(Vec2i(i, i + 1));
+ }
+
+ stringGeometry->initialize(verticesPtr, segmentsPtr);
+ return stringGeometry;
+}
+
+///
+/// \brief Create pbd string object
+///
+static std::shared_ptr<PbdObject>
+makePbdString(
+ const std::string& name,
+ const Vec3d& pos, const Vec3d& dir, const int numVerts,
+ const double stringLength)
+{
+ imstkNew<PbdObject> stringObj(name);
+
+ // Setup the Geometry
+ const double dx = stringLength / numVerts;
+ std::shared_ptr<LineMesh> stringMesh = makeStringGeometry(pos, dir * dx, numVerts);
+
+ // Setup the Parameters
+ imstkNew<PBDModelConfig> pbdParams;
+ pbdParams->enableConstraint(PbdConstraint::Type::Distance, 100.0);
+ pbdParams->enableBendConstraint(100000.0, 1);
+ pbdParams->enableBendConstraint(100000.0, 2);
+ pbdParams->m_fixedNodeIds = { 0, 1 };
+ pbdParams->m_uniformMassValue = 0.002 / numVerts; // grams
+ pbdParams->m_gravity = Vec3d(0.0, -9.8, 0.0);
+ pbdParams->m_dt = 0.0005; // Overwritten for real time
+
+ // Requires large amounts of iterations the longer, a different
+ // solver would help
+ pbdParams->m_iterations = 100;
+ pbdParams->m_viscousDampingCoeff = 0.01;
+
+ // Setup the Model
+ imstkNew<PbdModel> pbdModel;
+ pbdModel->setModelGeometry(stringMesh);
+ pbdModel->configure(pbdParams);
+
+ // Setup the VisualModel
+ imstkNew<RenderMaterial> material;
+ material->setBackFaceCulling(false);
+ material->setColor(Color::Red);
+ material->setLineWidth(2.0);
+ material->setPointSize(6.0);
+ material->setDisplayMode(RenderMaterial::DisplayMode::Wireframe);
+
+ imstkNew<VisualModel> visualModel(stringMesh);
+ visualModel->setRenderMaterial(material);
+
+ // Setup the Object
+ stringObj->addVisualModel(visualModel);
+ stringObj->setPhysicsGeometry(stringMesh);
+ stringObj->setCollidingGeometry(stringMesh);
+ stringObj->setDynamicalModel(pbdModel);
+
+ return stringObj;
+}
+
+///
+/// \brief Generate a static/immovable tissue for static suturing
+///
+static std::shared_ptr<CollidingObject>
+makeTissueObj()
+{
+ imstkNew<CollidingObject> tissueObj("tissue");
+
+ imstkNew<OrientedBox> box1(Vec3d(0.0, -0.1, -0.1), Vec3d(0.1, 0.025, 0.1));
+ imstkNew<VisualModel> box1Model;
+ box1Model->setGeometry(box1);
+ box1Model->getRenderMaterial()->setShadingModel(RenderMaterial::ShadingModel::Gouraud);
+ box1Model->getRenderMaterial()->setColor(Color::LightSkin);
+ tissueObj->addVisualModel(box1Model);
+
+ tissueObj->setCollidingGeometry(box1);
+
+ imstkNew<OrientedBox> box2(Vec3d(0.0, -0.105, -0.1), Vec3d(0.1001, 0.025, 0.1001));
+ imstkNew<VisualModel> box2Model;
+ box2Model->setGeometry(box2);
+ box2Model->getRenderMaterial()->setShadingModel(RenderMaterial::ShadingModel::Gouraud);
+ box2Model->getRenderMaterial()->setColor(Color::darken(Color::Yellow, 0.2));
+ tissueObj->addVisualModel(box2Model);
+
+ return tissueObj;
+}
+
+static std::shared_ptr<SceneObject>
+makeToolObj()
+{
+ auto surfMesh =
+ MeshIO::read<SurfaceMesh>(iMSTK_DATA_ROOT "/Surgical Instruments/Clamps/Gregory Suture Clamp/gregory_suture_clamp.obj");
+
+ auto toolObj = std::make_shared<SceneObject>("Clamps");
+ toolObj->setVisualGeometry(surfMesh);
+ auto renderMaterial = std::make_shared<RenderMaterial>();
+ renderMaterial->setColor(Color::LightGray);
+ renderMaterial->setShadingModel(RenderMaterial::ShadingModel::PBR);
+ renderMaterial->setRoughness(0.5);
+ renderMaterial->setMetalness(1.0);
+ toolObj->getVisualModel(0)->setRenderMaterial(renderMaterial);
+
+ return toolObj;
+}
+
+///
+/// \brief This example is an initial suturing example testbed. It provides the constraint
+/// required for an arc shaped needle puncturing vs a static/immovable tissue. What it
+/// does not do:
+/// - The tissue is not deformable yet, so insertion is a bit stiff
+/// - It only constrains the arc to the surface point it punctures not the volume
+/// - The suture thread isn't constrained yet
+/// - Ability to graps/release the needle, combining into one body
+///
+int
+main()
+{
+ // Setup logger (write to file and stdout)
+ Logger::startLogger();
+
+ imstkNew<Scene> scene("PBDStaticSuture");
+
+ imstkNew<NeedleObject> needleObj;
+ needleObj->setForceThreshold(2.0);
+ scene->addSceneObject(needleObj);
+
+ const double stringLength = 0.2;
+ const int stringVertexCount = 30;
+ std::shared_ptr<PbdObject> sutureThreadObj =
+ makePbdString("SutureThread", Vec3d(0.0, 0.0, 0.018), Vec3d(0.0, 0.0, 1.0),
+ stringVertexCount, stringLength);
+ scene->addSceneObject(sutureThreadObj);
+
+ std::shared_ptr<CollidingObject> tissueObj = makeTissueObj();
+ scene->addSceneObject(tissueObj);
+
+ std::shared_ptr<SceneObject> clampsObj = makeToolObj();
+ scene->addSceneObject(clampsObj);
+
+ auto interaction = std::make_shared<PbdObjectCollision>(sutureThreadObj, tissueObj, "ImplicitGeometryToPointSetCD");
+ interaction->setFriction(0.0);
+ scene->getCollisionGraph()->addInteraction(interaction);
+
+ auto needleInteraction = std::make_shared<NeedleInteraction>(tissueObj, needleObj);
+ scene->getCollisionGraph()->addInteraction(needleInteraction);
+
+ // Adjust the camera
+ scene->getActiveCamera()->setFocalPoint(0.00138345, -0.0601133, -0.0261938);
+ scene->getActiveCamera()->setPosition(0.00137719, 0.0492882, 0.201508);
+ scene->getActiveCamera()->setViewUp(-0.000780726, 0.901361, -0.433067);
+
+ // Run the simulation
+ {
+ // Setup a viewer to render
+ imstkNew<VTKViewer> viewer("Viewer");
+ viewer->setActiveScene(scene);
+ viewer->setDebugAxesLength(0.01, 0.01, 0.01);
+
+ // Setup a scene manager to advance the scene
+ imstkNew<SceneManager> sceneManager("Scene Manager");
+ sceneManager->setActiveScene(scene);
+ sceneManager->setExecutionType(Module::ExecutionType::ADAPTIVE);
+ sceneManager->pause(); // Start simulation paused
+
+ imstkNew<SimulationManager> driver;
+ driver->addModule(viewer);
+ driver->addModule(sceneManager);
+ driver->setDesiredDt(0.001); // 1ms, 1000hz
+
+#ifdef iMSTK_USE_OPENHAPTICS
+ imstkNew<HapticDeviceManager> hapticManager;
+ std::shared_ptr<HapticDeviceClient> hapticDeviceClient = hapticManager->makeDeviceClient();
+ driver->addModule(hapticManager);
+
+ imstkNew<RigidObjectController> controller(needleObj, hapticDeviceClient);
+ controller->setTranslationOffset(Vec3d(0.05, -0.05, 0.0));
+ controller->setTranslationScaling(0.001);
+ controller->setLinearKs(1000.0);
+ controller->setLinearKd(50.0);
+ controller->setAngularKs(10000000.0);
+ controller->setAngularKd(1000000.0);
+ controller->setForceScaling(0.2);
+ controller->setSmoothingKernelSize(5);
+ controller->setUseForceSmoothening(true);
+ scene->addController(controller);
+#endif
+
+ connect<Event>(sceneManager, &SceneManager::preUpdate,
+ [&](Event*)
+ {
+ //needleObj->getRigidBodyModel2()->getConfig()->m_dt = sceneManager->getDt();
+ sutureThreadObj->getPbdModel()->getParameters()->m_dt = sceneManager->getDt();
+ });
+ Vec3d trans = Vec3d(-0.009, 0.01, 0.001);
+ connect<Event>(sceneManager, &SceneManager::postUpdate,
+ [&](Event*)
+ {
+ // Constrain the first two vertices of the string to the needle
+ auto needleLineMesh = std::dynamic_pointer_cast<LineMesh>(needleObj->getPhysicsGeometry());
+ auto sutureLineMesh = std::dynamic_pointer_cast<LineMesh>(sutureThreadObj->getPhysicsGeometry());
+ (*sutureLineMesh->getVertexPositions())[1] = (*needleLineMesh->getVertexPositions())[0];
+ (*sutureLineMesh->getVertexPositions())[0] = (*needleLineMesh->getVertexPositions())[1];
+
+ // Transform the clamps relative to the needle
+ clampsObj->getVisualGeometry()->setTransform(
+ needleObj->getVisualGeometry()->getTransform() *
+ mat4dTranslate(trans) *
+ mat4dRotation(Rotd(PI, Vec3d(0.0, 1.0, 0.0))));
+ clampsObj->getVisualGeometry()->postModified();
+ });
+
+ // 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->setModuleDriver(driver);
+ viewer->addControl(keyControl);
+ }
+
+ driver->start();
+ }
+
+ return 0;
+}
\ No newline at end of file
diff --git a/Examples/PBD/PBDString/pbdStringExample.cpp b/Examples/PBD/PBDString/pbdStringExample.cpp
deleted file mode 100644
index c7a10279be12fd8775827be10dd82389fdf3437a..0000000000000000000000000000000000000000
--- a/Examples/PBD/PBDString/pbdStringExample.cpp
+++ /dev/null
@@ -1,231 +0,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 "imstkKeyboardSceneControl.h"
-#include "imstkLineMesh.h"
-#include "imstkLogger.h"
-#include "imstkMouseSceneControl.h"
-#include "imstkNew.h"
-#include "imstkPbdModel.h"
-#include "imstkPbdObject.h"
-#include "imstkRenderMaterial.h"
-#include "imstkScene.h"
-#include "imstkSceneManager.h"
-#include "imstkSimulationManager.h"
-#include "imstkVisualModel.h"
-#include "imstkVTKViewer.h"
-
-using namespace imstk;
-
-///
-/// \brief Create pbd string geometry
-///
-static std::shared_ptr<LineMesh>
-makeStringGeometry(const Vec3d& pos, const int numVerts, const double stringLength)
-{
- // Create the geometry
- imstkNew<LineMesh> stringGeometry;
-
- imstkNew<VecDataArray<double, 3>> verticesPtr(numVerts);
- VecDataArray<double, 3>& vertices = *verticesPtr.get();
- const double vertexSpacing = stringLength / numVerts;
- for (int i = 0; i < numVerts; i++)
- {
- vertices[i] = pos - Vec3d(0.0, static_cast<double>(i) * vertexSpacing, 0.0);
- }
-
- // Add connectivity data
- imstkNew<VecDataArray<int, 2>> segmentsPtr;
- VecDataArray<int, 2>& segments = *segmentsPtr.get();
- for (int i = 0; i < numVerts - 1; i++)
- {
- segments.push_back(Vec2i(i, i + 1));
- }
-
- stringGeometry->initialize(verticesPtr, segmentsPtr);
- return stringGeometry;
-}
-
-///
-/// \brief Create pbd string object
-///
-static std::shared_ptr<PbdObject>
-makePbdString(
- const std::string& name,
- const Vec3d& pos,
- const int numVerts,
- const double stringLength,
- const double bendStiffness,
- const Color& color)
-{
- imstkNew<PbdObject> stringObj(name);
-
- // Setup the Geometry
- std::shared_ptr<LineMesh> stringMesh = makeStringGeometry(pos, numVerts, stringLength);
-
- // Setup the Parameters
- imstkNew<PBDModelConfig> pbdParams;
- pbdParams->enableConstraint(PbdConstraint::Type::Distance, 1.0e7);
- pbdParams->enableConstraint(PbdConstraint::Type::Bend, bendStiffness);
- pbdParams->m_fixedNodeIds = { 0 };
- pbdParams->m_uniformMassValue = 5.0;
- pbdParams->m_gravity = Vec3d(0.0, -9.8, 0.0);
- pbdParams->m_dt = 0.0005;
- pbdParams->m_iterations = 5;
-
- // Setup the Model
- imstkNew<PbdModel> pbdModel;
- pbdModel->setModelGeometry(stringMesh);
- pbdModel->configure(pbdParams);
-
- // Setup the VisualModel
- imstkNew<RenderMaterial> material;
- material->setBackFaceCulling(false);
- material->setColor(color);
- material->setLineWidth(2.0f);
- material->setPointSize(6.0f);
- material->setDisplayMode(RenderMaterial::DisplayMode::Wireframe);
-
- imstkNew<VisualModel> visualModel(stringMesh);
- visualModel->setRenderMaterial(material);
-
- // Setup the Object
- stringObj->addVisualModel(visualModel);
- stringObj->setPhysicsGeometry(stringMesh);
- stringObj->setDynamicalModel(pbdModel);
-
- return stringObj;
-}
-
-static std::vector<std::shared_ptr<PbdObject>>
-makePbdStrings(const size_t numStrings,
- const int numVerts,
- const double stringSpacing,
- const double stringLength,
- const Color& startColor,
- const Color& endColor)
-{
- std::vector<std::shared_ptr<PbdObject>> pbdStringObjs(numStrings);
-
- const double size = stringSpacing * (numStrings - 1);
-
- for (unsigned int i = 0; i < numStrings; i++)
- {
- const Vec3d tipPos = Vec3d(static_cast<double>(i) * stringSpacing - size * 0.5, stringLength * 0.5, 0.0);
- const double t = static_cast<double>(i) / (numStrings - 1);
-
- pbdStringObjs[i] = makePbdString(
- "String " + std::to_string(i),
- tipPos,
- numVerts,
- stringLength,
- (static_cast<double>(i) * 0.1 / numStrings + 0.001) * 1e6,
- Color::lerpRgb(startColor, endColor, t));
- }
-
- return pbdStringObjs;
-}
-
-const double dt = 0.0005;
-const double radius = 1.5;
-const size_t numStrings = 8; // Number of strings
-const int numVerts = 30; // Number of vertices on each string
-const double stringSpacing = 2.0; // How far each string is apart
-const double stringLength = 10.0; // Total length of string
-const Color startColor = Color(1.0, 0.0, 0.0); // Color of first string
-const Color endColor = Color(0.0, 1.0, 0.0); // Color of last string
-
-///
-/// \brief This example demonstrates string simulation
-/// using Position based dynamics with varying bend stiffnesses
-///
-int
-main()
-{
- // Setup logger (write to file and stdout)
- Logger::startLogger();
-
- imstkNew<Scene> scene("PBDString");
-
- // Setup N separate strings with varying bend stiffnesses
- std::vector<std::shared_ptr<PbdObject>> pbdStringObjs =
- makePbdStrings(numStrings, numVerts, stringSpacing, stringLength, startColor, endColor);
- for (auto obj : pbdStringObjs)
- {
- scene->addSceneObject(obj);
- }
-
- // Adjust the camera
- scene->getActiveCamera()->setFocalPoint(0.0, 0.0, 0.0);
- scene->getActiveCamera()->setPosition(0.0, 0.0, 15.0);
-
- // Move the points every frame
- double t = 0.0;
- auto movePoints =
- [&pbdStringObjs, &t](Event*)
- {
- for (size_t i = 0; i < pbdStringObjs.size(); i++)
- {
- std::shared_ptr<PbdModel> model = pbdStringObjs[i]->getPbdModel();
- std::shared_ptr<VecDataArray<double, 3>> positions = model->getCurrentState()->getPositions();
- (*positions)[0] += Vec3d(
- -std::sin(t) * radius * dt,
- 0.0,
- std::cos(t) * radius * dt);
- }
- t += dt;
- };
-
- // Run the simulation
- {
- // Setup a viewer to render
- imstkNew<VTKViewer> viewer("Viewer");
- viewer->setActiveScene(scene);
-
- // Setup a scene manager to advance the scene
- imstkNew<SceneManager> sceneManager("Scene Manager");
- sceneManager->setActiveScene(scene);
- sceneManager->pause(); // Start simulation paused
-
- connect<Event>(sceneManager, &SceneManager::postUpdate, movePoints);
-
- imstkNew<SimulationManager> driver;
- driver->addModule(viewer);
- driver->addModule(sceneManager);
-
- // 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->setModuleDriver(driver);
- viewer->addControl(keyControl);
- }
-
- driver->start();
- }
-
- return 0;
-}
\ No newline at end of file
diff --git a/Examples/PBD/PBDTissueSurfaceNeedleContact/CMakeLists.txt b/Examples/PBD/PBDTissueSurfaceNeedleContact/CMakeLists.txt
index 66296b0626e8c3c37eb842b6846eba61a119374a..c1ce25b8723e34020e640eaff9407489094e4e11 100644
--- a/Examples/PBD/PBDTissueSurfaceNeedleContact/CMakeLists.txt
+++ b/Examples/PBD/PBDTissueSurfaceNeedleContact/CMakeLists.txt
@@ -26,8 +26,8 @@ imstk_add_executable(${PROJECT_NAME}
NeedleRigidBodyCH.h
NeedlePbdCH.h
NeedleObject.h
- RbdAngularNeedleLockingConstraint.h
- RbdLinearNeedleLockingConstraint.h
+ RbdAngularLockingConstraint.h
+ RbdAxesLockingConstraint.h
NeedleInteraction.h)
#-----------------------------------------------------------------------------
diff --git a/Examples/PBD/PBDTissueSurfaceNeedleContact/NeedleInteraction.h b/Examples/PBD/PBDTissueSurfaceNeedleContact/NeedleInteraction.h
index 839ba7a055cc698bac3fe49507e1be5d2c1a6822..d6c52a661e5af083db18a98131218eb025c317f8 100644
--- a/Examples/PBD/PBDTissueSurfaceNeedleContact/NeedleInteraction.h
+++ b/Examples/PBD/PBDTissueSurfaceNeedleContact/NeedleInteraction.h
@@ -61,16 +61,4 @@ public:
}
~NeedleInteraction() override = default;
-
-public:
- ///
- /// \brief Set the force threshold for the needle
- ///
- void setForceThreshold(const double forceThreshold)
- {
- if (auto needleRbdCh = std::dynamic_pointer_cast<NeedleRigidBodyCH>(getCollisionHandlingB()))
- {
- needleRbdCh->setNeedleForceThreshold(forceThreshold);
- }
- }
};
\ No newline at end of file
diff --git a/Examples/PBD/PBDTissueSurfaceNeedleContact/NeedleObject.h b/Examples/PBD/PBDTissueSurfaceNeedleContact/NeedleObject.h
index 22781bcf4567c0d4d679a786b3c83dd9b28ef2ad..e87b2e743623574d4419b55f1f9b784e10625d72 100644
--- a/Examples/PBD/PBDTissueSurfaceNeedleContact/NeedleObject.h
+++ b/Examples/PBD/PBDTissueSurfaceNeedleContact/NeedleObject.h
@@ -27,6 +27,14 @@ using namespace imstk;
class NeedleObject : public RigidObject2
{
+public:
+ enum class CollisionState
+ {
+ REMOVED,
+ TOUCHING,
+ INSERTED
+ };
+
public:
NeedleObject(const std::string& name) : RigidObject2(name) { }
virtual ~NeedleObject() = default;
@@ -34,17 +42,24 @@ public:
virtual const std::string getTypeName() const override { return "NeedleObject"; }
public:
- void setInserted(const bool inserted) { m_inserted = inserted; }
- bool getInserted() const { return m_inserted; }
+ void setCollisionState(const CollisionState state) { m_collisionState = state; }
+ CollisionState getCollisionState() const { return m_collisionState; }
+
+ ///
+ /// \brief Set the force threshold for the needle
+ ///
+ void setForceThreshold(const double forceThreshold) { m_forceThreshold = forceThreshold; }
+ double getForceThreshold() const { return m_forceThreshold; }
///
- /// \brief Returns the axes of the needle (tip-tail)
+ /// \brief Returns the current axes of the needle (tip-tail)
///
- const Vec3d getNeedleAxes() const
+ const Vec3d getAxes() const
{
return (-getCollidingGeometry()->getRotation().col(1)).normalized();
}
protected:
- bool m_inserted = false;
+ CollisionState m_collisionState = CollisionState::REMOVED;
+ double m_forceThreshold = 10.0;
};
\ No newline at end of file
diff --git a/Examples/PBD/PBDTissueSurfaceNeedleContact/NeedlePbdCH.h b/Examples/PBD/PBDTissueSurfaceNeedleContact/NeedlePbdCH.h
index 0c1a50430b8cefc8d157ccda5244680b006799fe..f0d53e5263b6bf0b9cd9bdee78fc34089ab4d7e2 100644
--- a/Examples/PBD/PBDTissueSurfaceNeedleContact/NeedlePbdCH.h
+++ b/Examples/PBD/PBDTissueSurfaceNeedleContact/NeedlePbdCH.h
@@ -26,6 +26,9 @@
using namespace imstk;
+///
+/// \brief Surface collision disabled upon puncture
+///
class NeedlePbdCH : public PBDCollisionHandling
{
public:
@@ -43,8 +46,8 @@ protected:
VertexMassPair ptB1, VertexMassPair ptB2, VertexMassPair ptB3,
double stiffnessA, double stiffnessB)
{
- auto needleObject = std::dynamic_pointer_cast<NeedleObject>(getInputObjectB());
- if (!needleObject->getInserted())
+ auto needleObj = std::dynamic_pointer_cast<NeedleObject>(getInputObjectB());
+ if (needleObj->getCollisionState() == NeedleObject::CollisionState::TOUCHING)
{
PBDCollisionHandling::addVTConstraint(ptA, ptB1, ptB2, ptB3, stiffnessA, stiffnessB);
}
@@ -58,8 +61,8 @@ protected:
VertexMassPair ptB1, VertexMassPair ptB2,
double stiffnessA, double stiffnessB)
{
- auto needleObject = std::dynamic_pointer_cast<NeedleObject>(getInputObjectB());
- if (!needleObject->getInserted())
+ auto needleObj = std::dynamic_pointer_cast<NeedleObject>(getInputObjectB());
+ if (needleObj->getCollisionState() == NeedleObject::CollisionState::TOUCHING)
{
PBDCollisionHandling::addEEConstraint(ptA1, ptA2, ptB1, ptB2, stiffnessA, stiffnessB);
}
@@ -73,8 +76,8 @@ protected:
VertexMassPair ptB1, VertexMassPair ptB2,
double stiffnessA, double stiffnessB)
{
- auto needleObject = std::dynamic_pointer_cast<NeedleObject>(getInputObjectB());
- if (!needleObject->getInserted())
+ auto needleObj = std::dynamic_pointer_cast<NeedleObject>(getInputObjectB());
+ if (needleObj->getCollisionState() == NeedleObject::CollisionState::TOUCHING)
{
PBDCollisionHandling::addPEConstraint(ptA1, ptB1, ptB2, stiffnessA, stiffnessB);
}
@@ -87,8 +90,8 @@ protected:
VertexMassPair ptA, VertexMassPair ptB,
double stiffnessA, double stiffnessB)
{
- auto needleObject = std::dynamic_pointer_cast<NeedleObject>(getInputObjectB());
- if (!needleObject->getInserted())
+ auto needleObj = std::dynamic_pointer_cast<NeedleObject>(getInputObjectB());
+ if (needleObj->getCollisionState() == NeedleObject::CollisionState::TOUCHING)
{
PBDCollisionHandling::addPPConstraint(ptA, ptB, stiffnessA, stiffnessB);
}
diff --git a/Examples/PBD/PBDTissueSurfaceNeedleContact/NeedleRigidBodyCH.h b/Examples/PBD/PBDTissueSurfaceNeedleContact/NeedleRigidBodyCH.h
index ac541541cc5d3eeb27da56ca8cc0af4277a7b558..0ed362206c66acf992d9a822a5199148707941b4 100644
--- a/Examples/PBD/PBDTissueSurfaceNeedleContact/NeedleRigidBodyCH.h
+++ b/Examples/PBD/PBDTissueSurfaceNeedleContact/NeedleRigidBodyCH.h
@@ -25,8 +25,8 @@
#include "imstkRbdContactConstraint.h"
#include "imstkRigidBodyModel2.h"
#include "NeedleObject.h"
-#include "RbdLinearNeedleLockingConstraint.h"
-#include "RbdAngularNeedleLockingConstraint.h"
+#include "RbdAxesLockingConstraint.h"
+#include "RbdAngularLockingConstraint.h"
using namespace imstk;
@@ -56,8 +56,8 @@ protected:
// If no collision, needle must be removed
if (elementsA.size() == 0)
{
- auto needleObject = std::dynamic_pointer_cast<NeedleObject>(getInputObjectA());
- needleObject->setInserted(false);
+ auto needleObj = std::dynamic_pointer_cast<NeedleObject>(getInputObjectA());
+ needleObj->setCollisionState(NeedleObject::CollisionState::REMOVED);
}
}
@@ -69,27 +69,35 @@ protected:
const Vec3d& contactPt, const Vec3d& contactNormal,
const double contactDepth) override
{
- auto needleObject = std::dynamic_pointer_cast<NeedleObject>(rbdObj);
- const Vec3d n = contactNormal.normalized();
-
- // Get all inwards force
- const Vec3d needleAxes = needleObject->getNeedleAxes();
- const double fN = std::max(needleAxes.dot(needleObject->getRigidBody()->getForce()), 0.0);
+ auto needleObj = std::dynamic_pointer_cast<NeedleObject>(rbdObj);
- // If the normal force exceeds 150 the needle may insert
- if (fN > m_needleForceThreshold && !needleObject->getInserted())
+ // If the normal force exceeds threshold the needle may insert
+ if (needleObj->getCollisionState() == NeedleObject::CollisionState::REMOVED)
{
- LOG(INFO) << "Puncture!\n";
- needleObject->setInserted(true);
+ needleObj->setCollisionState(NeedleObject::CollisionState::TOUCHING);
+ }
- // Record the axes to constrain too
- m_initNeedleAxes = needleAxes;
- m_initNeedleOrientation = Quatd(needleObject->getCollidingGeometry()->getRotation());
- m_initContactPt = contactPt;
+ const Vec3d n = contactNormal.normalized();
+ if (needleObj->getCollisionState() == NeedleObject::CollisionState::TOUCHING)
+ {
+ // Get all inwards force
+ const Vec3d needleAxes = needleObj->getAxes();
+ const double fN = std::max(needleAxes.dot(needleObj->getRigidBody()->getForce()), 0.0);
+
+ if (fN > m_needleForceThreshold)
+ {
+ LOG(INFO) << "Puncture!\n";
+ needleObj->setCollisionState(NeedleObject::CollisionState::INSERTED);
+
+ // Record the axes to constrain too
+ m_initNeedleAxes = needleAxes;
+ m_initNeedleOrientation = Quatd(needleObj->getCollidingGeometry()->getRotation());
+ m_initContactPt = contactPt;
+ }
}
// Only add contact normal constraint if not inserted
- if (!needleObject->getInserted())
+ if (needleObj->getCollisionState() == NeedleObject::CollisionState::TOUCHING)
{
auto contactConstraint = std::make_shared<RbdContactConstraint>(
rbdObj->getRigidBody(), nullptr,
@@ -100,32 +108,20 @@ protected:
rbdObj->getRigidBodyModel2()->addConstraint(contactConstraint);
}
// Lock to the initial axes when the needle is inserted
- else
+ else if (needleObj->getCollisionState() == NeedleObject::CollisionState::INSERTED)
{
- auto needleLockConstraint = std::make_shared<RbdLinearNeedleLockingConstraint>(
+ auto needleLockConstraint = std::make_shared<RbdAxesLockingConstraint>(
rbdObj->getRigidBody(),
m_initContactPt, m_initNeedleAxes,
0.05);
needleLockConstraint->compute(rbdObj->getRigidBodyModel2()->getTimeStep());
rbdObj->getRigidBodyModel2()->addConstraint(needleLockConstraint);
- auto needleLockConstraint2 = std::make_shared<RbdAngularNeedleLockingConstraint>(
+ auto needleLockConstraint2 = std::make_shared<RbdAngularLockingConstraint>(
rbdObj->getRigidBody(), m_initNeedleOrientation, 0.05);
needleLockConstraint2->compute(rbdObj->getRigidBodyModel2()->getTimeStep());
rbdObj->getRigidBodyModel2()->addConstraint(needleLockConstraint2);
}
-
- /*if (m_useFriction)
- {
- std::shared_ptr<RbdFrictionConstraint> frictionConstraint =
- std::make_shared<RbdFrictionConstraint>(
- rbdObj->getRigidBody(), nullptr,
- contactPt, contactNormal.normalized(), contactDepth,
- m_frictionalCoefficient,
- RbdConstraint::Side::A);
- frictionConstraint->compute(rbdObj->getRigidBodyModel2()->getTimeStep());
- rbdObj->getRigidBodyModel2()->addConstraint(frictionConstraint);
- }*/
}
protected:
diff --git a/Examples/PBD/PBDTissueSurfaceNeedleContact/PBDTissueSurfaceNeedleContactExample.cpp b/Examples/PBD/PBDTissueSurfaceNeedleContact/PBDTissueSurfaceNeedleContactExample.cpp
index f55533f8280de58232613db7526a03cf10e8076a..f3623c3bc300f62ddb8325f22c91a23f0b188c07 100644
--- a/Examples/PBD/PBDTissueSurfaceNeedleContact/PBDTissueSurfaceNeedleContactExample.cpp
+++ b/Examples/PBD/PBDTissueSurfaceNeedleContact/PBDTissueSurfaceNeedleContactExample.cpp
@@ -50,7 +50,7 @@
#include "imstkHapticDeviceClient.h"
#include "imstkRigidObjectController.h"
#else
-#include "imstkKeyboardDeviceClient.h"
+#include "imstkMouseDeviceClient.h"
#endif
using namespace imstk;
@@ -322,6 +322,7 @@ main()
scene->addSceneObject(tissueObj);
std::shared_ptr<NeedleObject> toolObj = makeToolObj();
+ toolObj->setForceThreshold(250.0);
scene->addSceneObject(toolObj);
// Setup a ghost tool object to show off virtual coupling
@@ -338,7 +339,6 @@ main()
scene->addSceneObject(ghostToolObj);
auto interaction = std::make_shared<NeedleInteraction>(tissueObj, toolObj);
- interaction->setForceThreshold(250.0);
scene->getCollisionGraph()->addInteraction(interaction);
// Light
@@ -382,34 +382,47 @@ main()
controller->setSmoothingKernelSize(20);
controller->setUseForceSmoothening(true);
scene->addController(controller);
+
+ connect<Event>(sceneManager, &SceneManager::postUpdate, [&](Event*)
+ {
+ ghostToolObj->getVisualModel(0)->getRenderMaterial()->setOpacity(std::min(1.0, controller->getForce().norm() / 15.0));
+
+ // Also apply controller transform to ghost geometry
+ toolGhostMesh->setTranslation(controller->getPosition());
+ toolGhostMesh->setRotation(controller->getRotation());
+ toolGhostMesh->updatePostTransformData();
+ toolGhostMesh->postModified();
+ });
#else
- // Use keyboard controls
- connect<Event>(sceneManager, SceneManager::preUpdate, [&](Event*)
+ connect<Event>(sceneManager, &SceneManager::postUpdate, [&](Event*)
{
- if (viewer->getKeyboardDevice()->getButton('k') == KEY_PRESS)
- {
- (*toolObj->getRigidBody()->m_pos) += Vec3d(0.0, -0.01, 0.0);
- }
- else if (viewer->getKeyboardDevice()->getButton('i') == KEY_PRESS)
- {
- (*toolObj->getRigidBody()->m_pos) += Vec3d(0.0, 0.01, 0.0);
- }
- else if (viewer->getKeyboardDevice()->getButton('j') == KEY_PRESS)
- {
- (*toolObj->getRigidBody()->m_pos) += Vec3d(-0.01, 0.0, 0.0);
- }
- else if (viewer->getKeyboardDevice()->getButton('l') == KEY_PRESS)
- {
- (*toolObj->getRigidBody()->m_pos) += Vec3d(0.01, 0.0, 0.0);
- }
- else if (viewer->getKeyboardDevice()->getButton('u') == KEY_PRESS)
- {
- (*toolObj->getRigidBody()->m_pos) += Vec3d(0.0, 0.0, -0.01);
- }
- else if (viewer->getKeyboardDevice()->getButton('o') == KEY_PRESS)
+ const Vec2d mousePos = viewer->getMouseDevice()->getPos();
+ const Vec3d desiredPos = Vec3d(mousePos[0] - 0.5, mousePos[1] - 0.5, 0.0) * 2.0 + Vec3d(0.0, 1.0, 0.0);
+ const Quatd desiredOrientation = Quatd(Rotd(0.0, Vec3d(1.0, 0.0, 0.0)));
+
+ Vec3d virtualForce;
{
- (*toolObj->getRigidBody()->m_pos) += Vec3d(0.0, 0.0, 0.01);
+ const Vec3d fS = (desiredPos - toolObj->getRigidBody()->getPosition()) * 1000.0; // Spring force
+ const Vec3d fD = -toolObj->getRigidBody()->getVelocity() * 100.0; // Spring damping
+
+ const Quatd dq = desiredOrientation * toolObj->getRigidBody()->getOrientation().inverse();
+ const Rotd angleAxes = Rotd(dq);
+ const Vec3d tS = angleAxes.axis() * angleAxes.angle() * 10000000.0;
+ const Vec3d tD = -toolObj->getRigidBody()->getAngularVelocity() * 1000.0;
+
+ virtualForce = fS + fD;
+ (*toolObj->getRigidBody()->m_force) += virtualForce;
+ (*toolObj->getRigidBody()->m_torque) += tS + tD;
}
+
+ // Update the ghost debug geometry
+ std::shared_ptr<Geometry> toolGhostMesh = ghostToolObj->getVisualGeometry();
+ toolGhostMesh->setRotation(desiredOrientation);
+ toolGhostMesh->setTranslation(desiredPos);
+ toolGhostMesh->updatePostTransformData();
+ toolGhostMesh->postModified();
+
+ ghostToolObj->getVisualModel(0)->getRenderMaterial()->setOpacity(std::min(1.0, virtualForce.norm() / 15.0));
});
#endif
@@ -418,22 +431,6 @@ main()
// Keep the tool moving in real time
toolObj->getRigidBodyModel2()->getConfig()->m_dt = sceneManager->getDt();
//tissueObj->getPbdModel()->getParameters()->m_dt = sceneManager->getDt();
-
-#ifdef iMSTK_USE_OPENHAPTICS
- if (controller->getForce().norm() > 1.0)
- {
- ghostToolObj->getVisualModel(0)->setIsVisible(true);
- }
- else
- {
- ghostToolObj->getVisualModel(0)->setIsVisible(false);
- }
- // Also apply controller transform to ghost geometry
- toolGhostMesh->setTranslation(controller->getPosition());
- toolGhostMesh->setRotation(controller->getRotation());
- toolGhostMesh->updatePostTransformData();
- toolGhostMesh->postModified();
-#endif
});
// Add mouse and keyboard controls to the viewer
diff --git a/Examples/PBD/PBDTissueVolumeNeedleContact/RbdAngularNeedleLockingConstraint.h b/Examples/PBD/PBDTissueSurfaceNeedleContact/RbdAngularLockingConstraint.h
similarity index 76%
rename from Examples/PBD/PBDTissueVolumeNeedleContact/RbdAngularNeedleLockingConstraint.h
rename to Examples/PBD/PBDTissueSurfaceNeedleContact/RbdAngularLockingConstraint.h
index 1b8eb1b0625a9841f0692b1ed5bab0408036f737..1cb1a10bc12d99dfe7195520b0b9e6291658dcc3 100644
--- a/Examples/PBD/PBDTissueVolumeNeedleContact/RbdAngularNeedleLockingConstraint.h
+++ b/Examples/PBD/PBDTissueSurfaceNeedleContact/RbdAngularLockingConstraint.h
@@ -26,23 +26,23 @@
using namespace imstk;
///
-/// \class RbdAngularNeedleLockingConstraint
+/// \class RbdAngularLockingConstraint
///
-/// \brief Constrains the orientation to some initial orientation
+/// \brief Constrains the orientation to some fixed orientation
///
-class RbdAngularNeedleLockingConstraint : public RbdConstraint
+class RbdAngularLockingConstraint : public RbdConstraint
{
public:
- RbdAngularNeedleLockingConstraint(
+ RbdAngularLockingConstraint(
std::shared_ptr<RigidBody> obj,
- const Quatd& initNeedleOrientation,
+ const Quatd& fixedOrientation,
const double beta = 0.05) : RbdConstraint(obj, nullptr, Side::A),
- m_initNeedleOrientation(initNeedleOrientation),
+ m_fixedOrientation(fixedOrientation),
m_beta(beta)
{
}
- ~RbdAngularNeedleLockingConstraint() override = default;
+ ~RbdAngularLockingConstraint() override = default;
public:
void compute(double dt) override
@@ -51,7 +51,7 @@ public:
J = Eigen::Matrix<double, 3, 4>::Zero();
if ((m_side == Side::AB || m_side == Side::A) && !m_obj1->m_isStatic)
{
- const Quatd dq = m_initNeedleOrientation * m_obj1->getOrientation().inverse();
+ const Quatd dq = m_fixedOrientation * m_obj1->getOrientation().inverse();
const Rotd angleAxes = Rotd(dq);
const Vec3d rotAxes = angleAxes.axis();
vu = angleAxes.angle() * m_beta / dt;
@@ -62,6 +62,6 @@ public:
}
private:
- Quatd m_initNeedleOrientation; ///> Orientation to fix too
+ Quatd m_fixedOrientation; ///> Orientation to fix too
double m_beta = 0.05;
};
\ No newline at end of file
diff --git a/Examples/PBD/PBDTissueSurfaceNeedleContact/RbdLinearNeedleLockingConstraint.h b/Examples/PBD/PBDTissueSurfaceNeedleContact/RbdAxesLockingConstraint.h
similarity index 67%
rename from Examples/PBD/PBDTissueSurfaceNeedleContact/RbdLinearNeedleLockingConstraint.h
rename to Examples/PBD/PBDTissueSurfaceNeedleContact/RbdAxesLockingConstraint.h
index 46ef84a5cb1f0601419e7c31476aa0980d2460f0..fd1485780d5ae5683682bb6ac6e2905f742fc0bc 100644
--- a/Examples/PBD/PBDTissueSurfaceNeedleContact/RbdLinearNeedleLockingConstraint.h
+++ b/Examples/PBD/PBDTissueSurfaceNeedleContact/RbdAxesLockingConstraint.h
@@ -23,33 +23,28 @@
#include "imstkRbdConstraint.h"
-#include <functional>
-
using namespace imstk;
///
-/// \class RbdLinearNeedleLockingConstraint
+/// \class RbdAxesLockingConstraint
///
-/// \brief Constrains the body to specified orientation
-/// and only allows linear movement along the inital axes
+/// \brief Constrains the body center of mass to a fixed axes
///
-class RbdLinearNeedleLockingConstraint : public RbdConstraint
+class RbdAxesLockingConstraint : public RbdConstraint
{
public:
- RbdLinearNeedleLockingConstraint(
+ RbdAxesLockingConstraint(
std::shared_ptr<RigidBody> obj,
- const Vec3d& initNeedleAxesPt,
- const Vec3d& initNeedleAxes,
- //const Quatd& initNeedleOrientation,
+ const Vec3d& axesPt,
+ const Vec3d& axesDir,
const double beta = 0.05) : RbdConstraint(obj, nullptr, Side::A),
- m_initNeedleAxesPt(initNeedleAxesPt),
- m_initNeedleAxes(initNeedleAxes),
- //m_initNeedleOrientation(initNeedleOrientation),
+ m_axesPt(axesPt),
+ m_axesDir(axesDir),
m_beta(beta)
{
}
- ~RbdLinearNeedleLockingConstraint() override = default;
+ ~RbdAxesLockingConstraint() override = default;
public:
void compute(double dt) override
@@ -58,9 +53,9 @@ public:
J = Eigen::Matrix<double, 3, 4>::Zero();
if ((m_side == Side::AB || m_side == Side::A) && !m_obj1->m_isStatic)
{
- // Displacement to needle Axes
- const Vec3d diff = m_obj1->getPosition() - m_initNeedleAxesPt;
- const Vec3d perpDisplacement = diff - m_initNeedleAxes.dot(diff) * m_initNeedleAxes;
+ // Displacement to needle Axes, constrain it to the axes
+ const Vec3d diff = m_obj1->getPosition() - m_axesPt;
+ const Vec3d perpDisplacement = diff - m_axesDir.dot(diff) * m_axesDir;
const double displacement = perpDisplacement.norm();
if (displacement != 0)
{
@@ -80,7 +75,7 @@ public:
}
private:
- Vec3d m_initNeedleAxesPt; ///> Point on the axes to constrain too
- Vec3d m_initNeedleAxes; //> Axes to constrain too
+ Vec3d m_axesPt; ///> Point on the axes to constrain too
+ Vec3d m_axesDir; //> Axes to constrain too
double m_beta = 0.05;
};
\ No newline at end of file
diff --git a/Examples/PBD/PBDTissueVolumeNeedleContact/CMakeLists.txt b/Examples/PBD/PBDTissueVolumeNeedleContact/CMakeLists.txt
index 9473e955ce447536bb346ac55916f4ae2d8e10ef..c5e4368fda65d9cda91b92d4313accc567049cfd 100644
--- a/Examples/PBD/PBDTissueVolumeNeedleContact/CMakeLists.txt
+++ b/Examples/PBD/PBDTissueVolumeNeedleContact/CMakeLists.txt
@@ -23,13 +23,15 @@ project(Example-PBDTissueVolumeNeedleContact)
#-----------------------------------------------------------------------------
imstk_add_executable(${PROJECT_NAME}
PBDTissueVolumeNeedleContactExample.cpp
- NeedleRigidBodyCH.h
- NeedlePbdCH.h
+ EmbeddingConstraint.h
+ EmbeddingConstraint.cpp
+ NeedleEmbeddedCH.h
+ NeedleEmbeddedCH.cpp
+ NeedleInteraction.h
+ NeedleInteraction.cpp
NeedleObject.h
- RbdAngularNeedleLockingConstraint.h
- RbdLinearNeedleLockingConstraint.h
- PbdTriangleEmbeddingConstraint.h
- NeedleInteraction.h)
+ NeedlePbdCH.h
+ NeedleRigidBodyCH.h)
#-----------------------------------------------------------------------------
# Add the target to Examples folder
@@ -39,4 +41,4 @@ SET_TARGET_PROPERTIES (${PROJECT_NAME} PROPERTIES FOLDER Examples/PBD)
#-----------------------------------------------------------------------------
# Link libraries to executable
#-----------------------------------------------------------------------------
-target_link_libraries(${PROJECT_NAME} SimulationManager)
+target_link_libraries(${PROJECT_NAME} SimulationManager Filtering)
diff --git a/Examples/PBD/PBDTissueVolumeNeedleContact/EmbeddingConstraint.cpp b/Examples/PBD/PBDTissueVolumeNeedleContact/EmbeddingConstraint.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9683244294e2b84b52afb50d8431896291c8dbe1
--- /dev/null
+++ b/Examples/PBD/PBDTissueVolumeNeedleContact/EmbeddingConstraint.cpp
@@ -0,0 +1,147 @@
+/*=========================================================================
+
+ 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 "EmbeddingConstraint.h"
+#include "imstkCollisionUtils.h"
+
+namespace imstk
+{
+// \todo: Try adding torque/rotation to the tool, two interpolant points a and b with line center of mass c
+// then rotation a to b around c is given with torques (a-c)x(b-c)
+
+void
+EmbeddingConstraint::initConstraint(
+ VertexMassPair ptB1, VertexMassPair ptB2, VertexMassPair ptB3,
+ Vec3d* p, Vec3d* q)
+{
+ // Set the triangle
+ m_bodiesSecond[0] = ptB1;
+ m_bodiesSecond[1] = ptB2;
+ m_bodiesSecond[2] = ptB3;
+ const Vec3d& x1 = *m_bodiesSecond[0].vertex;
+ const Vec3d& x2 = *m_bodiesSecond[1].vertex;
+ const Vec3d& x3 = *m_bodiesSecond[2].vertex;
+
+ // Compute intersection point & interpolant on triangle
+ CollisionUtils::testSegmentTriangle(*p, *q, x1, x2, x3, m_uvw);
+ m_iPt = x1 * m_uvw[0] + x2 * m_uvw[1] + x3 * m_uvw[2];
+ m_iPtVel = Vec3d::Zero();
+
+ // Set the point on the line for PBD
+ m_bodiesFirst[0] = { &m_iPt, 0.0, &m_iPtVel };
+ const Vec3d& x0 = *m_bodiesFirst[0].vertex;
+
+ // Completely rigid for PBD
+ m_stiffnessA = m_stiffnessB = 1.0;
+
+ // Compute the interpolant on the line
+ {
+ m_p = p;
+ m_q = q;
+ const Vec3d pq = (*p - *q).normalized();
+ const Vec3d d = x0 - *q;
+ m_t = pq.dot(d);
+ }
+}
+
+Vec3d
+EmbeddingConstraint::computeInterpolantDifference() const
+{
+ //const Vec3d& x0 = *m_bodiesFirst[0].vertex;
+ const Vec3d& x1 = *m_bodiesSecond[0].vertex;
+ const Vec3d& x2 = *m_bodiesSecond[1].vertex;
+ const Vec3d& x3 = *m_bodiesSecond[2].vertex;
+
+ Vec3d* p = m_p;
+ Vec3d* q = m_q;
+ const Vec3d pq = (*p - *q);
+ const Vec3d pq_n = pq.normalized();
+
+ // Compute the location of the intersection point on both elements
+ const Vec3d triPos = x1 * m_uvw[0] + x2 * m_uvw[1] + x3 * m_uvw[2];
+ const Vec3d linePos = (*q) + pq_n * m_t;
+
+ // Compute the transform to align the triangle to the line
+ return triPos - linePos;
+}
+
+bool
+EmbeddingConstraint::computeValueAndGradient(double& c,
+ std::vector<Vec3d>& dcdxA,
+ std::vector<Vec3d>& dcdxB) const
+{
+ //const Vec3d& x0 = *m_bodiesFirst[0].vertex;
+ //const Vec3d& x1 = *m_bodiesSecond[0].vertex;
+ //const Vec3d& x2 = *m_bodiesSecond[1].vertex;
+ //const Vec3d& x3 = *m_bodiesSecond[2].vertex;
+
+ // Compute the normal/axes of the line
+ const Vec3d pq = *m_p - *m_q;
+ const Vec3d pq_n = pq.normalized();
+
+ // Compute the difference between the two interpolated points on the elements
+ Vec3d diff = computeInterpolantDifference();
+
+ // Remove any normal movement (remove only fraction for sort of friction)
+ // Frees normal movement
+ diff = diff - diff.dot(pq_n) * pq_n * (1.0 - m_normalFriction);
+ const Vec3d n = diff.normalized();
+
+ dcdxA[0] = Vec3d::Zero();
+ dcdxB[0] = n;
+ dcdxB[1] = n;
+ dcdxB[2] = n;
+
+ c = -diff.norm() * (1.0 - m_compliance);
+
+ return true;
+}
+
+void
+EmbeddingConstraint::compute(double dt)
+{
+ // Jacobian of contact (defines linear and angular constraint axes)
+ J = Eigen::Matrix<double, 3, 4>::Zero();
+ if (!m_obj1->m_isStatic)
+ {
+ // Compute the normal/axes of the line
+ const Vec3d pq = *m_p - *m_q;
+ const Vec3d pq_n = pq.normalized();
+
+ // Compute the difference between the two interpolated points on the elements
+ Vec3d diff = computeInterpolantDifference();
+
+ // Remove any normal movement (remove only fraction for sort of friction)
+ // Frees normal movement
+ diff = diff - diff.dot(pq_n) * pq_n * (1.0 - m_normalFriction);
+ const Vec3d n = -diff.normalized();
+
+ vu = diff.norm() * m_beta / dt * m_compliance;
+
+ // Displacement from center of mass
+ J(0, 0) = -n[0]; J(0, 1) = 0.0;
+ J(1, 0) = -n[1]; J(1, 1) = 0.0;
+ J(2, 0) = -n[2]; J(2, 1) = 0.0;
+ }
+}
+}
\ No newline at end of file
diff --git a/Examples/PBD/PBDTissueVolumeNeedleContact/EmbeddingConstraint.h b/Examples/PBD/PBDTissueVolumeNeedleContact/EmbeddingConstraint.h
new file mode 100644
index 0000000000000000000000000000000000000000..95c16e56be5200ed8362d16d412b96f395cbbab3
--- /dev/null
+++ b/Examples/PBD/PBDTissueVolumeNeedleContact/EmbeddingConstraint.h
@@ -0,0 +1,119 @@
+/*=========================================================================
+
+ 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 "imstkPbdCollisionConstraint.h"
+#include "imstkRbdConstraint.h"
+
+namespace imstk
+{
+///
+/// \brief Given a line and a triangle it computes the intersection between
+/// them upon initialation for which it saves the interpolation weights.
+///
+/// This allows us to deform both shapes whilst maintaining a relative
+/// position on that element.
+///
+/// To then constrain we compute the difference between the two interpolated
+/// positions on each element and pull the line back towards the triangle
+/// via rbd constraint and pull the triangle back towards the line via pbd
+///
+/// A compliance term gives the weighting for which to do this. To make more
+/// physically accurate we would need to take an approach like XPBD in solving.
+///
+class EmbeddingConstraint : public PbdCollisionConstraint, public RbdConstraint
+{
+public:
+ EmbeddingConstraint(std::shared_ptr<RigidBody> obj1) :
+ PbdCollisionConstraint(1, 3), RbdConstraint(obj1, nullptr, RbdConstraint::Side::A)
+ {
+ }
+
+ ~EmbeddingConstraint() override = default;
+
+public:
+ Type getType() const { return Type::PointTriangle; }
+
+ ///
+ /// \brief Initializes both PBD and RBD constraint
+ ///
+ void initConstraint(
+ VertexMassPair ptB1, VertexMassPair ptB2, VertexMassPair ptB3,
+ Vec3d* p, Vec3d* q);
+
+public:
+ ///
+ /// \brief Given two interpolants on the two elements, compute the difference
+ /// between them and use for resolution
+ ///
+ Vec3d computeInterpolantDifference() const;
+
+public:
+ ///
+ /// \brief Update the pbd constraint
+ ///
+ bool computeValueAndGradient(double& c,
+ std::vector<Vec3d>& dcdxA,
+ std::vector<Vec3d>& dcdxB) const override;
+
+public:
+ ///
+ /// \brief Update the rbd constraint
+ ///
+ void compute(double dt) override;
+
+protected:
+ // Intersection point via interpolants on triangle
+ Vec3d m_uvw = Vec3d::Zero();
+ // Intersection point via interpolants on line
+ double m_t = 0.0;
+
+ // The actual line
+ Vec3d* m_p = nullptr;
+ Vec3d* m_q = nullptr;
+
+// The triangle is stored in PBD base class
+
+protected:
+ // Step for rbd constraint
+ double m_beta = 0.05;
+
+ Vec3d m_iPt;
+ Vec3d m_iPtVel;
+
+protected:
+ // Ratio between the two models (ie: how much rbd tool is moved vs how much pbd tissue is)
+ //
+ // If 0.0, rbd tool is completely resolved and PBD tissue does not move
+ // If 1.0, pbd tissue completely moves and rbd tool feels no resistance
+ double m_compliance = 0.5;
+
+ //// Ratio between the two models. This gives how much the rbd tool is moved vs how much pbd tissue is
+ //// in the normal direction
+ //double m_normalCompliance = 0.5;
+
+ // If 0.0, completely removes pbd reaction in line axes direction, the pbd triangle will completely let
+ // the tool slide in that direction
+ // If 1.0, completely resist normal movement
+ double m_normalFriction = 0.0;
+};
+}
\ No newline at end of file
diff --git a/Examples/PBD/PBDTissueVolumeNeedleContact/NeedleEmbeddedCH.cpp b/Examples/PBD/PBDTissueVolumeNeedleContact/NeedleEmbeddedCH.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..60897c6dfa067db31d7fb013fc022835f739fd36
--- /dev/null
+++ b/Examples/PBD/PBDTissueVolumeNeedleContact/NeedleEmbeddedCH.cpp
@@ -0,0 +1,252 @@
+/*=========================================================================
+
+ 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 "NeedleEmbeddedCH.h"
+#include "EmbeddingConstraint.h"
+#include "imstkCollisionUtils.h"
+#include "imstkLineMesh.h"
+#include "imstkPbdObject.h"
+#include "imstkPbdSolver.h"
+#include "imstkRigidBodyModel2.h"
+#include "imstkTetrahedralMesh.h"
+#include "NeedleObject.h"
+
+#include "imstkMeshIO.h"
+
+#include <unordered_set>
+
+using namespace imstk;
+
+NeedleEmbeddedCH::NeedleEmbeddedCH() :
+ m_solver(std::make_shared<PbdCollisionSolver>())
+{
+}
+
+std::shared_ptr<Geometry>
+NeedleEmbeddedCH::getHandlingGeometryA()
+{
+ auto tissueObj = std::dynamic_pointer_cast<PbdObject>(getInputObjectA());
+ return (tissueObj == nullptr) ? nullptr : tissueObj->getPhysicsGeometry();
+}
+
+void
+NeedleEmbeddedCH::correctVelocities()
+{
+ for (int i = 0; i < m_solverConstraints.size(); i++)
+ {
+ m_solverConstraints[i]->correctVelocity(m_friction, 1.0);
+ }
+}
+
+void
+NeedleEmbeddedCH::solve()
+{
+ m_solver->solve();
+}
+
+void
+NeedleEmbeddedCH::handle(
+ const std::vector<CollisionElement>& elementsA,
+ const std::vector<CollisionElement>& elementsB)
+{
+ auto tissueObj = std::dynamic_pointer_cast<PbdObject>(getInputObjectA());
+ auto needleObj = std::dynamic_pointer_cast<NeedleObject>(getInputObjectB());
+
+ // If needle needle is touching the surface
+ if (needleObj->getCollisionState() == NeedleObject::CollisionState::TOUCHING)
+ {
+ // Get force along the needle axes
+ const Vec3d needleAxes = needleObj->getNeedleAxes();
+ const double fN = std::max(needleAxes.dot(needleObj->getRigidBody()->getForce()), 0.0);
+
+ // If the normal force exceeds the threshold, mark needle as inserted
+ if (fN > needleObj->getForceThreshold())
+ {
+ LOG(INFO) << "Puncture!";
+ needleObj->setCollisionState(NeedleObject::CollisionState::INSERTED);
+ }
+ }
+
+ // If the needle is inserted
+ if (needleObj->getCollisionState() == NeedleObject::CollisionState::INSERTED)
+ {
+ // Check if there are no tet intersections. If none, mark removed
+ if (elementsA.size() == 0)
+ {
+ LOG(INFO) << "Unpunctured!";
+ needleObj->setCollisionState(NeedleObject::CollisionState::REMOVED);
+ m_faceConstraints.clear();
+ }
+ }
+ // If the needle is not inserted
+ else
+ {
+ // Don't run the embedded CH
+ return;
+ }
+
+ auto tissueGeom = std::dynamic_pointer_cast<TetrahedralMesh>(tissueObj->getPhysicsGeometry());
+ auto needleGeom = std::dynamic_pointer_cast<LineMesh>(needleObj->getCollidingGeometry());
+ needleGeom->updatePostTransformData();
+
+ std::shared_ptr<VecDataArray<double, 3>> tissueVerticesPtr = tissueGeom->getVertexPositions();
+ std::shared_ptr<VecDataArray<int, 4>> tissueIndicesPtr = tissueGeom->getTetrahedraIndices();
+
+ auto tissueVelocitiesPtr = std::dynamic_pointer_cast<VecDataArray<double, 3>>(tissueGeom->getVertexAttribute("Velocities"));
+ auto tissueInvMassesPtr = std::dynamic_pointer_cast<DataArray<double>>(tissueGeom->getVertexAttribute("InvMass"));
+
+ VecDataArray<double, 3>& tissueVertices = *tissueVerticesPtr;
+ const VecDataArray<int, 4>& tissueIndices = *tissueIndicesPtr;
+ VecDataArray<double, 3>& tissueVelocities = *tissueVelocitiesPtr;
+ const DataArray<double>& tissueInvMasses = *tissueInvMassesPtr;
+
+ std::shared_ptr<VecDataArray<double, 3>> needleVerticesPtr = needleGeom->getVertexPositions();
+ std::shared_ptr<VecDataArray<int, 2>> needleIndicesPtr = needleGeom->getLinesIndices();
+ VecDataArray<double, 3>& needleVertices = *needleVerticesPtr;
+ const VecDataArray<int, 2>& needleIndices = *needleIndicesPtr;
+
+ // Keep track of the constraints that are added *this iteration* vs those already present
+ // in m_faceConstraints so we can find the set that are no longer present
+ std::unordered_set<std::shared_ptr<EmbeddingConstraint>> m_constraintEnabled;
+
+ // Constrain the triangle to the intersection point
+ // If constraint for triangle already exists, update existing intersection point
+ m_debugEmbeddingPoints.clear();
+ m_debugEmbeddedTriangles.clear();
+ auto addConstraint =
+ [&](int v1, int v2, int v3, const Vec3d& iPt)
+ {
+ // Hashable triangle (to resolve shared triangles, any order of v1,v2,v3 maps to same constraint)
+ TriCell triCell(v1, v2, v3);
+
+ m_debugEmbeddingPoints.push_back(iPt);
+ m_debugEmbeddedTriangles.push_back(Vec3i(v1, v2, v3));
+
+ // If constraint doesn't already exist for this triangle
+ if (m_faceConstraints.count(triCell) == 0)
+ {
+ auto constraint = std::make_shared<EmbeddingConstraint>(needleObj->getRigidBody());
+
+ constraint->initConstraint(
+ { &tissueVertices[v1], tissueInvMasses[v1], &tissueVelocities[v1] },
+ { &tissueVertices[v2], tissueInvMasses[v2], &tissueVelocities[v2] },
+ { &tissueVertices[v3], tissueInvMasses[v3], &tissueVelocities[v3] },
+ &needleVertices[0], &needleVertices[1]);
+
+ // Add the constraint to a map of face->constraint
+ m_faceConstraints[triCell] = constraint;
+ //printf("Adding embedding constraint at %d, %d, %d\n", v1, v2, v3);
+ }
+
+ // Mark as present
+ m_constraintEnabled.insert(m_faceConstraints[triCell]);
+ };
+
+ // For every intersected element
+ for (int i = 0; i < elementsA.size(); i++)
+ {
+ const CollisionElement& colElemA = elementsA[i];
+ const CollisionElement& colElemB = elementsB[i];
+
+ if (colElemA.m_type != CollisionElementType::CellIndex || colElemB.m_type != CollisionElementType::CellIndex)
+ {
+ continue;
+ }
+
+ const CellIndexElement& elemA = colElemA.m_element.m_CellIndexElement;
+ const CellIndexElement& elemB = colElemB.m_element.m_CellIndexElement;
+
+ if (elemA.cellType == IMSTK_TETRAHEDRON && elemB.cellType == IMSTK_EDGE)
+ {
+ Vec4i tet;
+ if (elemA.idCount == 1)
+ {
+ tet = tissueIndices[elemA.ids[0]];
+ }
+ else if (elemA.idCount == 4)
+ {
+ tet = Vec4i(elemA.ids[0], elemA.ids[1], elemA.ids[2], elemA.ids[3]);
+ }
+
+ std::array<Vec3d, 2> lineVerts;
+ if (elemB.idCount == 1)
+ {
+ const Vec2i& line = needleIndices[elemB.ids[0]];
+ lineVerts[0] = needleVertices[line[0]];
+ lineVerts[1] = needleVertices[line[1]];
+ }
+ else if (elemB.idCount == 2)
+ {
+ lineVerts[0] = needleVertices[elemB.ids[0]];
+ lineVerts[1] = needleVertices[elemB.ids[1]];
+ }
+ const double lineLength = (lineVerts[1] - lineVerts[0]).norm();
+
+ // For every face of the tet
+ static int faces[4][3] = { { 0, 1, 2 }, { 1, 2, 3 }, { 0, 2, 3 }, { 0, 1, 3 } };
+ for (int j = 0; j < 4; j++)
+ {
+ // Find intersection point and add constraints
+ const Vec3d& a = tissueVertices[tet[faces[j][0]]];
+ const Vec3d& b = tissueVertices[tet[faces[j][1]]];
+ const Vec3d& c = tissueVertices[tet[faces[j][2]]];
+
+ Vec3d iPt;
+ if (CollisionUtils::testPlaneLine(lineVerts[0], lineVerts[1], a, (b - a).cross(c - a).normalized(), iPt))
+ {
+ Vec3d uvw = baryCentric(iPt, a, b, c);
+ if (uvw[0] >= 0.0 && uvw[1] >= 0.0 && uvw[2] >= 0.0)
+ {
+ // If within line bounds
+ const double t = (lineVerts[1] - lineVerts[0]).normalized().dot(iPt - lineVerts[0]);
+ if (t > 0.0 && t < lineLength)
+ {
+ addConstraint(
+ tet[faces[j][0]],
+ tet[faces[j][1]],
+ tet[faces[j][2]],
+ iPt);
+ }
+ }
+ }
+ }
+ }
+ }
+
+ // Add constraint to the PBD solver and RBD system
+ m_solverConstraints.resize(0);
+ m_solverConstraints.reserve(m_faceConstraints.size());
+ for (auto i = m_faceConstraints.begin(); i != m_faceConstraints.end(); i++)
+ {
+ if (m_constraintEnabled.count(i->second) != 0)
+ {
+ // Add pbd and rbd constraint
+ m_solverConstraints.push_back(i->second.get());
+ i->second->compute(needleObj->getRigidBodyModel2()->getTimeStep());
+ needleObj->getRigidBodyModel2()->addConstraint(i->second);
+ }
+ else
+ {
+ i = m_faceConstraints.erase(i);
+ }
+ }
+ m_solver->addCollisionConstraints(&m_solverConstraints);
+}
\ No newline at end of file
diff --git a/Examples/PBD/PBDTissueVolumeNeedleContact/NeedleEmbeddedCH.h b/Examples/PBD/PBDTissueVolumeNeedleContact/NeedleEmbeddedCH.h
index ecb1fd43d0e013ed5f1cf76c3ba2f7d365d329c3..d5c703bf348d2f0185fb682bcd01f3ba353c00a7 100644
--- a/Examples/PBD/PBDTissueVolumeNeedleContact/NeedleEmbeddedCH.h
+++ b/Examples/PBD/PBDTissueVolumeNeedleContact/NeedleEmbeddedCH.h
@@ -22,20 +22,15 @@
#pragma once
#include "imstkCollisionHandling.h"
-#include "imstkPbdCollisionConstraint.h"
-#include "imstkCollisionUtils.h"
-#include "PbdTriangleEmbeddingConstraint.h"
#include <unordered_map>
namespace imstk
{
+class EmbeddingConstraint;
+class Geometry;
class PbdCollisionSolver;
-class PbdEdgeEdgeConstraint;
-class PbdObject;
-class PbdPointEdgeConstraint;
-class PbdPointPointConstraint;
-class PbdPointTriangleConstraint;
+class PbdCollisionConstraint;
}
using namespace imstk;
@@ -96,231 +91,54 @@ struct hash<TriCell>
///
/// \class NeedleEmbeddedCH
///
-/// \brief Implements PBD-RBD embedded tissue handling
+/// \brief Implements PBD-RBD embedded tissue handling for when the
+/// needle is embedded in the tissue
///
class NeedleEmbeddedCH : public CollisionHandling
{
public:
- NeedleEmbeddedCH() :
- m_solver(std::make_shared<PbdCollisionSolver>())
- {
- }
-
+ NeedleEmbeddedCH();
virtual ~NeedleEmbeddedCH() override = default;
virtual const std::string getTypeName() const override { return "NeedleEmbeddedCH"; }
public:
- std::shared_ptr<Geometry> getHandlingGeometryA() override
- {
- auto tissueObj = std::dynamic_pointer_cast<PbdObject>(getInputObjectA());
- return (tissueObj == nullptr) ? nullptr : tissueObj->getPhysicsGeometry();
- }
+ std::shared_ptr<Geometry> getHandlingGeometryA() override;
public:
///
/// \brief Corrects for velocity (restitution and friction) after PBD is complete
///
- void correctVelocities()
- {
- for (int i = 0; i < m_solverConstraints.size(); i++)
- {
- m_solverConstraints[i]->correctVelocity(m_friction, 1.0);
- }
- }
+ void correctVelocities();
- void solve() { m_solver->solve(); }
+ void solve();
void setFriction(const double friction) { m_friction = friction; }
const double getFriction() const { return m_friction; }
+ void setCollisionSolver(std::shared_ptr<PbdCollisionSolver> solver) { m_solver = solver; }
+ std::shared_ptr<PbdCollisionSolver> getCollisionSolver() const { return m_solver; }
+
protected:
///
/// \brief Add embedding constraints based off contact data
+ /// We need to add the constraint once and then update it later
///
void handle(
const std::vector<CollisionElement>& elementsA,
- const std::vector<CollisionElement>& elementsB) override
- {
- auto tissueObj = std::dynamic_pointer_cast<PbdObject>(getInputObjectA());
- auto needleObj = std::dynamic_pointer_cast<NeedleObject>(getInputObjectB());
-
- auto tissueGeom = std::dynamic_pointer_cast<TetrahedralMesh>(tissueObj->getPhysicsGeometry());
- auto needleGeom = std::dynamic_pointer_cast<LineMesh>(needleObj->getCollidingGeometry());
-
- std::shared_ptr<VecDataArray<double, 3>> tissueVerticesPtr = tissueGeom->getVertexPositions();
- std::shared_ptr<VecDataArray<int, 4>> tissueIndicesPtr = tissueGeom->getTetrahedraIndices();
- auto tissueVelocitiesPtr = std::dynamic_pointer_cast<VecDataArray<double, 3>>(tissueGeom->getVertexAttribute("Velocities"));
- auto tissueInvMassesPtr = std::dynamic_pointer_cast<DataArray<double>>(tissueGeom->getVertexAttribute("InvMass"));
-
- VecDataArray<double, 3>& tissueVertices = *tissueVerticesPtr;
- const VecDataArray<int, 4>& tissueIndices = *tissueIndicesPtr;
- VecDataArray<double, 3>& tissueVelocities = *tissueVelocitiesPtr;
- const DataArray<double>& tissueInvMasses = *tissueInvMassesPtr;
-
- std::shared_ptr<VecDataArray<double, 3>> needleVerticesPtr = needleGeom->getVertexPositions();
- std::shared_ptr<VecDataArray<int, 2>> needleIndicesPtr = needleGeom->getLinesIndices();
- VecDataArray<double, 3>& needleVertices = *needleVerticesPtr;
- const VecDataArray<int, 2>& needleIndices = *needleIndicesPtr;
-
- // Keep track of the constraints that are added *this iteration* vs those already present
- // so we can find the set that are no longer present
- std::unordered_set<std::shared_ptr<PbdTriangleEmbeddingConstraint>> m_constraintEnabled;
- auto isConstraintEnabled = [&](std::shared_ptr<PbdTriangleEmbeddingConstraint> constraint)
- {
- return m_constraintEnabled.count(constraint) != 0;
- };
-
- // Constrain the triangle to the intersection point
- // If constraint for triangle already exists, update existing intersection point
- auto addConstraint = [&](int v1, int v2, int v3, const Vec3d& iPt)
- {
- // Hashable triangle
- TriCell triCell(v1, v2, v3);
-
- // If constraint doesn't exist OR if constraint exists but has been disabled
- if (m_faceConstraints.count(triCell) == 0 || m_faceConstraints[triCell] == nullptr)
- {
- auto constraint = std::make_shared<PbdTriangleEmbeddingConstraint>();
-
- // Push back to a list so mem locations don't change
- m_fixedPoints.push_back(iPt);
- m_fixedPointVelocities.push_back(Vec3d::Zero());
-
- constraint->initConstraint(
- { &m_fixedPoints.back(), 0.0, &m_fixedPointVelocities.back() },
- { &tissueVertices[v1], tissueInvMasses[v1], &tissueVelocities[v1] },
- { &tissueVertices[v2], tissueInvMasses[v2], &tissueVelocities[v2] },
- { &tissueVertices[v3], tissueInvMasses[v3], &tissueVelocities[v3] },
- 1.0, 1.0, &needleVertices[0], &needleVertices[1], m_friction);
-
- // Add the constraint to a map of face->constraint
- m_faceConstraints[triCell] = constraint;
- m_constraintEnabled.insert(constraint);
- }
- // If already contains, then update the intersection point
- else
- {
- // Get the existing constraint
- std::shared_ptr<PbdTriangleEmbeddingConstraint> constraint = m_faceConstraints[triCell];
- (*constraint->getVertexMassA().vertex) = iPt;
- m_constraintEnabled.insert(constraint);
- }
- };
-
- // If needle has been inserted
- if (needleObj->getInserted())
- {
- if (elementsA.size() != elementsB.size())
- {
- return;
- }
-
- // Tet faces
- static int faces[4][3] = { { 0, 1, 2 }, { 1, 2, 3 }, { 0, 2, 3 }, { 0, 1, 3 } };
-
- // For every intersected element
- for (int i = 0; i < elementsA.size(); i++)
- {
- const CollisionElement& colElemA = elementsA[i];
- const CollisionElement& colElemB = elementsB[i];
-
- if (colElemA.m_type != CollisionElementType::CellIndex || colElemB.m_type != CollisionElementType::CellIndex)
- {
- continue;
- }
-
- const CellIndexElement& elemA = colElemA.m_element.m_CellIndexElement;
- const CellIndexElement& elemB = colElemB.m_element.m_CellIndexElement;
-
- if (elemA.cellType == IMSTK_TETRAHEDRON && elemB.cellType == IMSTK_EDGE)
- {
- Vec4i tet;
- if (elemA.idCount == 1)
- {
- tet = tissueIndices[elemA.ids[0]];
- }
- else if (elemA.idCount == 4)
- {
- tet = Vec4i(elemA.ids[0], elemA.ids[1], elemA.ids[2], elemA.ids[3]);
- }
- std::array<Vec3d, 4> tetVerts;
- tetVerts[0] = tissueVertices[tet[0]];
- tetVerts[1] = tissueVertices[tet[1]];
- tetVerts[2] = tissueVertices[tet[2]];
- tetVerts[3] = tissueVertices[tet[3]];
-
- std::array<Vec3d, 2> lineVerts;
- if (elemB.idCount == 1)
- {
- const Vec2i& line = needleIndices[elemB.ids[0]];
- lineVerts[0] = needleVertices[line[0]];
- lineVerts[1] = needleVertices[line[1]];
- }
- else if (elemB.idCount == 2)
- {
- lineVerts[0] = needleVertices[elemB.ids[0]];
- lineVerts[1] = needleVertices[elemB.ids[1]];
- }
-
- // Determine which faces intersect
- int face0 = -1;
- int face1 = -1;
- Vec3d iPt0 = Vec3d::Zero();
- Vec3d iPt1 = Vec3d::Zero();
- if (CollisionUtils::testTetToSegment(tetVerts,
- lineVerts[0], lineVerts[1],
- face0, face1, iPt0, iPt1))
- {
- if (face0 != -1)
- {
- addConstraint(
- tet[faces[face0][0]],
- tet[faces[face0][1]],
- tet[faces[face0][2]],
- iPt0);
- }
- if (face1 != -1)
- {
- addConstraint(
- tet[faces[face1][0]],
- tet[faces[face1][1]],
- tet[faces[face1][2]],
- iPt1);
- }
- }
- }
- }
- }
-
- m_solverConstraints.resize(0);
- m_solverConstraints.reserve(m_faceConstraints.size());
- for (auto& i : m_faceConstraints)
- {
- if (i.second != nullptr && isConstraintEnabled(i.second))
- {
- m_solverConstraints.push_back(i.second.get());
- }
- else
- {
- // Cleanup (without deleting to avoid slight cost)
- i.second = nullptr;
- }
- }
- m_solver->addCollisionConstraints(&m_solverConstraints);
- }
+ const std::vector<CollisionElement>& elementsB) override;
private:
// TriCell takes care of duplicate faces
- std::unordered_map<TriCell, std::shared_ptr<PbdTriangleEmbeddingConstraint>> m_faceConstraints;
+ std::unordered_map<TriCell, std::shared_ptr<EmbeddingConstraint>> m_faceConstraints;
std::shared_ptr<PbdCollisionSolver> m_solver = nullptr;
std::vector<PbdCollisionConstraint*> m_solverConstraints; ///> List of PBD constraints
- // Lists to hold valid references to them in memory
- std::list<Vec3d> m_fixedPoints;
- std::list<Vec3d> m_fixedPointVelocities;
-
//double m_restitution = 0.0; ///> Coefficient of restitution (1.0 = perfect elastic, 0.0 = inelastic)
double m_friction = 0.001; ///> Coefficient of friction (1.0 = full frictional force, 0.0 = none)
+
+public:
+ std::vector<Vec3d> m_debugEmbeddingPoints; ///> Used for debug visualization
+ std::vector<Vec3i> m_debugEmbeddedTriangles;
};
\ No newline at end of file
diff --git a/Examples/PBD/PBDTissueVolumeNeedleContact/NeedleInteraction.cpp b/Examples/PBD/PBDTissueVolumeNeedleContact/NeedleInteraction.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..24cc125c0ec98e01ab7978f42dbdbaadd73f8388
--- /dev/null
+++ b/Examples/PBD/PBDTissueVolumeNeedleContact/NeedleInteraction.cpp
@@ -0,0 +1,123 @@
+/*=========================================================================
+
+ 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 "NeedleInteraction.h"
+#include "imstkLineMesh.h"
+#include "imstkPbdObject.h"
+#include "imstkTaskGraph.h"
+#include "imstkTetraToLineMeshCD.h"
+#include "NeedleEmbeddedCH.h"
+#include "NeedlePbdCH.h"
+#include "NeedleRigidBodyCH.h"
+#include "imstkTetrahedralMesh.h"
+
+using namespace imstk;
+
+NeedleInteraction::NeedleInteraction(std::shared_ptr<PbdObject> tissueObj,
+ std::shared_ptr<NeedleObject> needleObj) : PbdRigidObjectCollision(tissueObj, needleObj)
+{
+ if (std::dynamic_pointer_cast<LineMesh>(needleObj->getCollidingGeometry()) == nullptr)
+ {
+ LOG(WARNING) << "NeedleInteraction only works with LineMesh collision geometry on rigid NeedleObject";
+ }
+ if (std::dynamic_pointer_cast<TetrahedralMesh>(tissueObj->getPhysicsGeometry()) == nullptr)
+ {
+ LOG(WARNING) << "NeedleInteraction only works with TetrahedralMesh physics geometry on pbd tissueObj";
+ }
+
+ // First replace the handlers with our needle subclasses
+
+ // This handler consumes collision data to resolve the tool from the tissue
+ // except when the needle is inserted
+ auto needleRbdCH = std::make_shared<NeedleRigidBodyCH>();
+ needleRbdCH->setInputRigidObjectA(needleObj);
+ needleRbdCH->setInputCollidingObjectB(tissueObj);
+ needleRbdCH->setInputCollisionData(getCollisionDetection()->getCollisionData());
+ needleRbdCH->setBeta(0.001);
+ needleRbdCH->getTaskNode()->m_isCritical = true;
+ setCollisionHandlingB(needleRbdCH);
+
+ // This handler consumes the collision data to resolve the tissue from the tool
+ // except when the needle is inserted
+ auto needlePbdCH = std::make_shared<NeedlePbdCH>();
+ needlePbdCH->setInputObjectA(tissueObj);
+ needlePbdCH->setInputObjectB(needleObj);
+ needlePbdCH->setInputCollisionData(getCollisionDetection()->getCollisionData());
+ //needlePbdCH->getCollisionSolver()->setCollisionIterations(1);
+ needlePbdCH->getTaskNode()->m_isCritical = true;
+ setCollisionHandlingA(needlePbdCH);
+
+ // Then add a separate scheme for when the needle is embedded
+
+ // Assumes usage of physics geometry for this
+ tetMeshCD = std::make_shared<TetraToLineMeshCD>();
+ tetMeshCD->setInputGeometryA(tissueObj->getPhysicsGeometry());
+ tetMeshCD->setInputGeometryB(needleObj->getCollidingGeometry());
+
+ embeddedCH = std::make_shared<NeedleEmbeddedCH>();
+ embeddedCH->setInputCollisionData(tetMeshCD->getCollisionData());
+ embeddedCH->setCollisionSolver(needlePbdCH->getCollisionSolver());
+ embeddedCH->setInputObjectA(tissueObj);
+ embeddedCH->setInputObjectB(needleObj);
+ embeddedCH->getTaskNode()->m_isCritical = true;
+}
+
+void
+NeedleInteraction::apply()
+{
+ PbdRigidObjectCollision::apply();
+
+ std::shared_ptr<TaskGraph> taskGraphA = m_objects.first->getTaskGraph(); // Pbd
+ std::shared_ptr<TaskGraph> taskGraphB = m_objects.second->getTaskGraph(); // Rbd
+
+ std::shared_ptr<CollisionDetectionAlgorithm> cd = m_colDetect;
+
+ auto pbdObj = std::dynamic_pointer_cast<PbdObject>(m_objects.first);
+ std::shared_ptr<CollisionHandling> pbdCH = m_colHandlingA;
+
+ auto rbdObj = std::dynamic_pointer_cast<RigidObject2>(m_objects.second);
+ std::shared_ptr<CollisionHandling> rbdCH = m_colHandlingB;
+
+ // Needle interaction introduces its own collision detection step, handling, solve, and velocity correction
+ auto needleEmbeddedCD =
+ std::make_shared<TaskNode>([&]() { tetMeshCD->update(); }, "NeedleEmbeddingCD", true);
+ auto embeddingCHNode =
+ std::make_shared<TaskNode>([&]() { embeddedCH->update(); }, "NeedleEmbeddingCH", true);
+
+ {
+ taskGraphA->addNode(needleEmbeddedCD);
+ taskGraphA->addNode(embeddingCHNode);
+
+ // PBD CH -> EmbeddedCD -> EmbeddedCH -> Collision Solve
+ taskGraphA->addEdge(m_colHandlingA->getTaskNode(), needleEmbeddedCD);
+ taskGraphA->addEdge(needleEmbeddedCD, embeddingCHNode);
+ taskGraphA->addEdge(embeddingCHNode, m_pbdCollisionSolveNode);
+ }
+
+ {
+ taskGraphB->addNode(needleEmbeddedCD);
+ taskGraphB->addNode(embeddingCHNode);
+
+ taskGraphB->addEdge(m_colHandlingB->getTaskNode(), needleEmbeddedCD);
+ taskGraphB->addEdge(needleEmbeddedCD, embeddingCHNode);
+ taskGraphB->addEdge(embeddingCHNode, rbdObj->getRigidBodyModel2()->getSolveNode());
+ }
+}
\ No newline at end of file
diff --git a/Examples/PBD/PBDTissueVolumeNeedleContact/NeedleInteraction.h b/Examples/PBD/PBDTissueVolumeNeedleContact/NeedleInteraction.h
index cb726402ec4e4f95257569bea1fc8f491ea7245b..4c12dba748d12adca2244e2ea9a79da8424c2ba7 100644
--- a/Examples/PBD/PBDTissueVolumeNeedleContact/NeedleInteraction.h
+++ b/Examples/PBD/PBDTissueVolumeNeedleContact/NeedleInteraction.h
@@ -22,16 +22,17 @@
#pragma once
#include "imstkPbdRigidObjectCollision.h"
-#include "imstkTetraToLineMeshCD.h"
-#include "NeedlePbdCH.h"
-#include "NeedleRigidBodyCH.h"
-#include "NeedleEmbeddedCH.h"
+using namespace imstk;
-#include "imstkTaskGraph.h"
-#include "imstkTaskNode.h"
+class NeedleEmbeddedCH;
+class NeedleObject;
-using namespace imstk;
+namespace imstk
+{
+class PbdObject;
+class TetraToLineMeshCD;
+}
///
/// \class NeedleInteraction
@@ -41,121 +42,16 @@ using namespace imstk;
class NeedleInteraction : public PbdRigidObjectCollision
{
public:
- NeedleInteraction(std::shared_ptr<PbdObject> tissueObj, std::shared_ptr<NeedleObject> needleObj) : PbdRigidObjectCollision(tissueObj, needleObj)
- {
- if (std::dynamic_pointer_cast<LineMesh>(needleObj->getCollidingGeometry()) == nullptr)
- {
- LOG(WARNING) << "NeedleInteraction only works with LineMesh collision geometry on NeedleObject";
- }
-
- // This handler consumes collision data to resolve the tool from the tissue
- imstkNew<NeedleRigidBodyCH> needleRbdCH;
- needleRbdCH->setInputRigidObjectA(needleObj);
- needleRbdCH->setInputCollidingObjectB(tissueObj);
- needleRbdCH->setInputCollisionData(getCollisionDetection()->getCollisionData());
- needleRbdCH->setBeta(0.001);
- needleRbdCH->getTaskNode()->m_isCritical = true;
- setCollisionHandlingB(needleRbdCH);
-
- // This handler consumes the collision data to resolve the tissue from the tool
- imstkNew<NeedlePbdCH> needlePbdCH;
- needlePbdCH->setInputObjectA(tissueObj);
- needlePbdCH->setInputObjectB(needleObj);
- needlePbdCH->setInputCollisionData(getCollisionDetection()->getCollisionData());
- needlePbdCH->getCollisionSolver()->setCollisionIterations(1);
- needlePbdCH->getTaskNode()->m_isCritical = true;
- setCollisionHandlingA(needlePbdCH);
-
- // This handler consumes collision data to produce PBD and RBD constraints when the tool
- // is embedded in the tissue (ie: when NeedleObject::inserted is on)
-
- // Assumes usage of physics geometry for this
- tetMeshCD = std::make_shared<TetraToLineMeshCD>();
- tetMeshCD->setInputGeometryA(tissueObj->getPhysicsGeometry());
- tetMeshCD->setInputGeometryB(needleObj->getCollidingGeometry());
-
- embeddedCH = std::make_shared<NeedleEmbeddedCH>();
- embeddedCH->setInputCollisionData(tetMeshCD->getCollisionData());
- embeddedCH->setInputObjectA(tissueObj);
- embeddedCH->setInputObjectB(needleObj);
- embeddedCH->getTaskNode()->m_isCritical = true;
- }
-
+ NeedleInteraction(std::shared_ptr<PbdObject> tissueObj,
+ std::shared_ptr<NeedleObject> needleObj);
~NeedleInteraction() override = default;
public:
- ///
- /// \brief Set the force threshold for the needle
- ///
- void setForceThreshold(const double forceThreshold)
- {
- if (auto needleRbdCh = std::dynamic_pointer_cast<NeedleRigidBodyCH>(getCollisionHandlingB()))
- {
- needleRbdCh->setNeedleForceThreshold(forceThreshold);
- }
- }
+ std::shared_ptr<TetraToLineMeshCD> getEmbeddingCD() const { return tetMeshCD; }
+ std::shared_ptr<NeedleEmbeddedCH> getEmbeddingCH() const { return embeddedCH; }
protected:
- void apply() override
- {
- PbdRigidObjectCollision::apply();
-
- std::shared_ptr<TaskGraph> taskGraphA = m_objects.first->getTaskGraph(); // Pbd
- std::shared_ptr<TaskGraph> taskGraphB = m_objects.second->getTaskGraph(); // Rbd
-
- std::shared_ptr<CollisionDetectionAlgorithm> cd = m_colDetect;
-
- auto pbdObj = std::dynamic_pointer_cast<PbdObject>(m_objects.first);
- std::shared_ptr<CollisionHandling> pbdCH = m_colHandlingA;
-
- auto rbdObj = std::dynamic_pointer_cast<RigidObject2>(m_objects.second);
- std::shared_ptr<CollisionHandling> rbdCH = m_colHandlingB;
-
- // Detection collision with the tet mesh
- auto needleEmbeddedCD =
- std::make_shared<TaskNode>([&]() { tetMeshCD->update(); }, "NeedleEmbeddingCD", true);
- taskGraphA->addNode(needleEmbeddedCD);
- taskGraphB->addNode(needleEmbeddedCD);
-
- taskGraphA->addEdge(m_colDetect->getTaskNode(), needleEmbeddedCD);
- taskGraphA->addEdge(needleEmbeddedCD, pbdCH->getTaskNode());
-
- taskGraphB->addEdge(m_colDetect->getTaskNode(), needleEmbeddedCD);
- taskGraphB->addEdge(needleEmbeddedCD, pbdCH->getTaskNode());
-
- // Consume the collision data after main handler executes but before collisions are solved for
- auto embeddingCHNode =
- std::make_shared<TaskNode>([&]() { embeddedCH->update(); }, "NeedleEmbeddingCH", true);
- taskGraphA->addNode(embeddingCHNode);
- taskGraphB->addNode(embeddingCHNode);
-
- // pbdCH -> embeddingCH -> collision solve (moves vertices)
- taskGraphA->addEdge(pbdCH->getTaskNode(), embeddingCHNode);
- taskGraphA->addEdge(embeddingCHNode, m_pbdCollisionSolveNode);
-
- taskGraphB->addEdge(pbdCH->getTaskNode(), embeddingCHNode);
- taskGraphB->addEdge(embeddingCHNode, m_pbdCollisionSolveNode);
-
- // Add a solver step for the embedding constraints solver
- // Should happen after collision to ensure convergence on final constraints
- auto embeddingSolveNode =
- std::make_shared<TaskNode>([&]() { embeddedCH->solve(); }, "NeedleEmbeddingSolve", true);
- auto correctNeedleVelocitiesNode =
- std::make_shared<TaskNode>([&]() { embeddedCH->correctVelocities(); }, "NeedleEmbeddingCorrectVelocities");
- taskGraphA->addNode(embeddingSolveNode);
- taskGraphA->addNode(correctNeedleVelocitiesNode);
- taskGraphB->addNode(embeddingSolveNode);
- taskGraphB->addNode(correctNeedleVelocitiesNode);
-
- taskGraphA->addEdge(m_pbdCollisionSolveNode, embeddingSolveNode);
- taskGraphA->addEdge(embeddingSolveNode, pbdObj->getPbdModel()->getUpdateVelocityNode());
-
- taskGraphA->addEdge(m_correctVelocitiesNode, correctNeedleVelocitiesNode);
- taskGraphA->addEdge(correctNeedleVelocitiesNode, pbdObj->getPbdModel()->getTaskGraph()->getSink());
-
- taskGraphB->addEdge(m_pbdCollisionSolveNode, embeddingSolveNode);
- taskGraphB->addEdge(embeddingSolveNode, rbdObj->getRigidBodyModel2()->getIntegrateNode());
- }
+ void apply() override;
protected:
std::shared_ptr<TetraToLineMeshCD> tetMeshCD;
diff --git a/Examples/PBD/PBDTissueVolumeNeedleContact/NeedleObject.h b/Examples/PBD/PBDTissueVolumeNeedleContact/NeedleObject.h
index 22781bcf4567c0d4d679a786b3c83dd9b28ef2ad..0e1998e8305b986bfc02f70d1bd39fed8cf95cc4 100644
--- a/Examples/PBD/PBDTissueVolumeNeedleContact/NeedleObject.h
+++ b/Examples/PBD/PBDTissueVolumeNeedleContact/NeedleObject.h
@@ -27,6 +27,14 @@ using namespace imstk;
class NeedleObject : public RigidObject2
{
+public:
+ enum class CollisionState
+ {
+ REMOVED,
+ TOUCHING,
+ INSERTED
+ };
+
public:
NeedleObject(const std::string& name) : RigidObject2(name) { }
virtual ~NeedleObject() = default;
@@ -34,11 +42,17 @@ public:
virtual const std::string getTypeName() const override { return "NeedleObject"; }
public:
- void setInserted(const bool inserted) { m_inserted = inserted; }
- bool getInserted() const { return m_inserted; }
+ void setCollisionState(const CollisionState state) { m_collisionState = state; }
+ CollisionState getCollisionState() const { return m_collisionState; }
+
+ ///
+ /// \brief Set the force threshold for the needle
+ ///
+ void setForceThreshold(const double forceThreshold) { m_forceThreshold = forceThreshold; }
+ double getForceThreshold() const { return m_forceThreshold; }
///
- /// \brief Returns the axes of the needle (tip-tail)
+ /// \brief Returns the current axes of the needle (tip-tail)
///
const Vec3d getNeedleAxes() const
{
@@ -46,5 +60,6 @@ public:
}
protected:
- bool m_inserted = false;
+ CollisionState m_collisionState = CollisionState::REMOVED;
+ double m_forceThreshold = 10.0;
};
\ No newline at end of file
diff --git a/Examples/PBD/PBDTissueVolumeNeedleContact/NeedlePbdCH.h b/Examples/PBD/PBDTissueVolumeNeedleContact/NeedlePbdCH.h
index 0c1a50430b8cefc8d157ccda5244680b006799fe..f0d53e5263b6bf0b9cd9bdee78fc34089ab4d7e2 100644
--- a/Examples/PBD/PBDTissueVolumeNeedleContact/NeedlePbdCH.h
+++ b/Examples/PBD/PBDTissueVolumeNeedleContact/NeedlePbdCH.h
@@ -26,6 +26,9 @@
using namespace imstk;
+///
+/// \brief Surface collision disabled upon puncture
+///
class NeedlePbdCH : public PBDCollisionHandling
{
public:
@@ -43,8 +46,8 @@ protected:
VertexMassPair ptB1, VertexMassPair ptB2, VertexMassPair ptB3,
double stiffnessA, double stiffnessB)
{
- auto needleObject = std::dynamic_pointer_cast<NeedleObject>(getInputObjectB());
- if (!needleObject->getInserted())
+ auto needleObj = std::dynamic_pointer_cast<NeedleObject>(getInputObjectB());
+ if (needleObj->getCollisionState() == NeedleObject::CollisionState::TOUCHING)
{
PBDCollisionHandling::addVTConstraint(ptA, ptB1, ptB2, ptB3, stiffnessA, stiffnessB);
}
@@ -58,8 +61,8 @@ protected:
VertexMassPair ptB1, VertexMassPair ptB2,
double stiffnessA, double stiffnessB)
{
- auto needleObject = std::dynamic_pointer_cast<NeedleObject>(getInputObjectB());
- if (!needleObject->getInserted())
+ auto needleObj = std::dynamic_pointer_cast<NeedleObject>(getInputObjectB());
+ if (needleObj->getCollisionState() == NeedleObject::CollisionState::TOUCHING)
{
PBDCollisionHandling::addEEConstraint(ptA1, ptA2, ptB1, ptB2, stiffnessA, stiffnessB);
}
@@ -73,8 +76,8 @@ protected:
VertexMassPair ptB1, VertexMassPair ptB2,
double stiffnessA, double stiffnessB)
{
- auto needleObject = std::dynamic_pointer_cast<NeedleObject>(getInputObjectB());
- if (!needleObject->getInserted())
+ auto needleObj = std::dynamic_pointer_cast<NeedleObject>(getInputObjectB());
+ if (needleObj->getCollisionState() == NeedleObject::CollisionState::TOUCHING)
{
PBDCollisionHandling::addPEConstraint(ptA1, ptB1, ptB2, stiffnessA, stiffnessB);
}
@@ -87,8 +90,8 @@ protected:
VertexMassPair ptA, VertexMassPair ptB,
double stiffnessA, double stiffnessB)
{
- auto needleObject = std::dynamic_pointer_cast<NeedleObject>(getInputObjectB());
- if (!needleObject->getInserted())
+ auto needleObj = std::dynamic_pointer_cast<NeedleObject>(getInputObjectB());
+ if (needleObj->getCollisionState() == NeedleObject::CollisionState::TOUCHING)
{
PBDCollisionHandling::addPPConstraint(ptA, ptB, stiffnessA, stiffnessB);
}
diff --git a/Examples/PBD/PBDTissueVolumeNeedleContact/NeedleRigidBodyCH.h b/Examples/PBD/PBDTissueVolumeNeedleContact/NeedleRigidBodyCH.h
index 03fb6a3ae3d85fded19d0d2a6e80564900fb6f15..2222ce38b75790b09a5c55a4e6e2e949bd1af14c 100644
--- a/Examples/PBD/PBDTissueVolumeNeedleContact/NeedleRigidBodyCH.h
+++ b/Examples/PBD/PBDTissueVolumeNeedleContact/NeedleRigidBodyCH.h
@@ -25,11 +25,12 @@
#include "imstkRbdContactConstraint.h"
#include "imstkRigidBodyModel2.h"
#include "NeedleObject.h"
-#include "RbdLinearNeedleLockingConstraint.h"
-#include "RbdAngularNeedleLockingConstraint.h"
using namespace imstk;
+///
+/// \brief Surface collision disabled upon puncture
+///
class NeedleRigidBodyCH : public RigidBodyCH
{
public:
@@ -38,29 +39,7 @@ public:
virtual const std::string getTypeName() const override { return "NeedleRigidBodyCH"; }
-public:
- void setNeedleForceThreshold(double needleForceThreshold) { m_needleForceThreshold = needleForceThreshold; }
- double getNeedleForceThrehsold() const { return m_needleForceThreshold; }
-
protected:
- ///
- /// \brief Handle the collision/contact data
- ///
- virtual void handle(
- const std::vector<CollisionElement>& elementsA,
- const std::vector<CollisionElement>& elementsB) override
- {
- // Do it the normal way
- RigidBodyCH::handle(elementsA, elementsB);
-
- // If no collision, needle must be removed
- if (elementsA.size() == 0)
- {
- auto needleObject = std::dynamic_pointer_cast<NeedleObject>(getInputObjectA());
- needleObject->setInserted(false);
- }
- }
-
///
/// \brief Add constraint for the rigid body given contact
///
@@ -69,28 +48,18 @@ protected:
const Vec3d& contactPt, const Vec3d& contactNormal,
const double contactDepth) override
{
- auto needleObject = std::dynamic_pointer_cast<NeedleObject>(rbdObj);
- const Vec3d n = contactNormal.normalized();
-
- // Get all inwards force
- const Vec3d needleAxes = needleObject->getNeedleAxes();
- const double fN = std::max(needleAxes.dot(needleObject->getRigidBody()->getForce()), 0.0);
+ auto needleObj = std::dynamic_pointer_cast<NeedleObject>(rbdObj);
- // If the normal force exceeds 150 the needle may insert
- if (fN > m_needleForceThreshold && !needleObject->getInserted())
+ if (needleObj->getCollisionState() == NeedleObject::CollisionState::REMOVED)
{
- LOG(INFO) << "Puncture!\n";
- needleObject->setInserted(true);
-
- // Record the axes to constrain too
- m_initNeedleAxes = needleAxes;
- m_initNeedleOrientation = Quatd(needleObject->getCollidingGeometry()->getRotation());
- m_initContactPt = contactPt;
+ needleObj->setCollisionState(NeedleObject::CollisionState::TOUCHING);
}
// Only add contact normal constraint if not inserted
- if (!needleObject->getInserted())
+ if (needleObj->getCollisionState() == NeedleObject::CollisionState::TOUCHING)
{
+ const Vec3d n = contactNormal.normalized();
+
auto contactConstraint = std::make_shared<RbdContactConstraint>(
rbdObj->getRigidBody(), nullptr,
n, contactPt, contactDepth,
@@ -99,39 +68,5 @@ protected:
contactConstraint->compute(rbdObj->getRigidBodyModel2()->getTimeStep());
rbdObj->getRigidBodyModel2()->addConstraint(contactConstraint);
}
- // Lock to the initial axes when the needle is inserted
- else
- {
- auto needleLockConstraint = std::make_shared<RbdLinearNeedleLockingConstraint>(
- rbdObj->getRigidBody(),
- &m_initContactPt, &m_initNeedleAxes,
- 0.05);
- needleLockConstraint->compute(rbdObj->getRigidBodyModel2()->getTimeStep());
- rbdObj->getRigidBodyModel2()->addConstraint(needleLockConstraint);
-
- auto needleLockConstraint2 = std::make_shared<RbdAngularNeedleLockingConstraint>(
- rbdObj->getRigidBody(), m_initNeedleOrientation, 0.05);
- needleLockConstraint2->compute(rbdObj->getRigidBodyModel2()->getTimeStep());
- rbdObj->getRigidBodyModel2()->addConstraint(needleLockConstraint2);
- }
-
- if (m_useFriction)
- {
- /*std::shared_ptr<RbdFrictionConstraint> frictionConstraint =
- std::make_shared<RbdFrictionConstraint>(
- rbdObj->getRigidBody(), nullptr,
- contactPt, contactNormal.normalized(), contactDepth,
- m_frictionalCoefficient,
- RbdConstraint::Side::A);
- frictionConstraint->compute(rbdObj->getRigidBodyModel2()->getTimeStep());
- rbdObj->getRigidBodyModel2()->addConstraint(frictionConstraint);*/
- }
}
-
-protected:
- double m_needleForceThreshold = 250.0; ///> When needle body exceeds this it inserts
-
- Vec3d m_initContactPt = Vec3d::Zero();
- Vec3d m_initNeedleAxes = Vec3d::Zero();
- Quatd m_initNeedleOrientation = Quatd::Identity();
};
\ No newline at end of file
diff --git a/Examples/PBD/PBDTissueVolumeNeedleContact/PBDTissueVolumeNeedleContactExample.cpp b/Examples/PBD/PBDTissueVolumeNeedleContact/PBDTissueVolumeNeedleContactExample.cpp
index e8ee92615dfdfe595cba81c536512901199e5be8..2cadb54a99d1c10c7a2a7b303af011597e5580b3 100644
--- a/Examples/PBD/PBDTissueVolumeNeedleContact/PBDTissueVolumeNeedleContactExample.cpp
+++ b/Examples/PBD/PBDTissueVolumeNeedleContact/PBDTissueVolumeNeedleContactExample.cpp
@@ -21,8 +21,8 @@
#include "imstkCamera.h"
#include "imstkCollisionGraph.h"
+#include "imstkDebugGeometryObject.h"
#include "imstkDirectionalLight.h"
-#include "imstkImageData.h"
#include "imstkIsometricMap.h"
#include "imstkKeyboardSceneControl.h"
#include "imstkLineMesh.h"
@@ -32,8 +32,9 @@
#include "imstkOneToOneMap.h"
#include "imstkPbdModel.h"
#include "imstkPbdObject.h"
-#include "imstkPbdSolver.h"
+#include "imstkRbdConstraint.h"
#include "imstkRenderMaterial.h"
+#include "imstkRigidBodyModel2.h"
#include "imstkScene.h"
#include "imstkSceneManager.h"
#include "imstkSimulationManager.h"
@@ -41,14 +42,16 @@
#include "imstkTetrahedralMesh.h"
#include "imstkVisualModel.h"
#include "imstkVTKViewer.h"
+#include "NeedleEmbeddedCH.h"
#include "NeedleInteraction.h"
+#include "NeedleObject.h"
#ifdef iMSTK_USE_OPENHAPTICS
#include "imstkHapticDeviceManager.h"
#include "imstkHapticDeviceClient.h"
#include "imstkRigidObjectController.h"
#else
-#include "imstkKeyboardDeviceClient.h"
+#include "imstkMouseDeviceClient.h"
#endif
using namespace imstk;
@@ -195,17 +198,31 @@ makeTissueObj(const std::string& name,
// Setup the Parameters
imstkNew<PBDModelConfig> pbdParams;
- // Use FEMTet constraints
- pbdParams->m_femParams->m_YoungModulus = 5.0;
- pbdParams->m_femParams->m_PoissonRatio = 0.4;
+ // Actual skin young's modulus, 0.42MPa to 0.85Mpa, as reported in papers
+ // Actual skin possion ratio, 0.48, as reported in papers
+ pbdParams->m_femParams->m_YoungModulus = 420000.0;
+ pbdParams->m_femParams->m_PoissonRatio = 0.48;
+ // FYI:
+ // - Poisson ratio gives shear to bulk, with 0.5 being complete shear
+ // where everything is like a fluid and can slide past each other. 0.0
+ // gives complete bulk where its rigid
+ // - Youngs modulus then gives the scaling of the above in pressure
+ // (pascals).
pbdParams->enableFEMConstraint(PbdConstraint::Type::FEMTet,
PbdFEMConstraint::MaterialType::StVK);
- pbdParams->m_doPartitioning = true;
- pbdParams->m_uniformMassValue = 0.1;
- pbdParams->m_gravity = Vec3d(0.0, 0.0, 0.0);
- pbdParams->m_dt = 0.05;
- pbdParams->m_iterations = 9;
- pbdParams->m_viscousDampingCoeff = 0.05;
+ pbdParams->m_doPartitioning = false;
+ pbdParams->m_uniformMassValue = 100.0;
+ pbdParams->m_dt = 0.001; // realtime used in update calls later in main
+ pbdParams->m_iterations = 5;
+
+ // Due to poor boundary conditions turning off gravity is useful. But then makes
+ // your tissue look like it's in space (springy and no resistance). So viscous
+ // damping is introduced to approximate these conditions.
+ //
+ // Ultimately this is a result of not modelling everything around the tissue.
+ // and poor/hard to model boundary conditions.
+ pbdParams->m_gravity = Vec3d::Zero();
+ pbdParams->m_viscousDampingCoeff = 0.03; // Removed from velocity
// Fix the borders
for (int z = 0; z < dim[2]; z++)
@@ -230,15 +247,15 @@ makeTissueObj(const std::string& name,
// Setup the material
imstkNew<RenderMaterial> material;
material->setDisplayMode(RenderMaterial::DisplayMode::WireframeSurface);
- material->setShadingModel(RenderMaterial::ShadingModel::PBR);
- auto diffuseTex = MeshIO::read<ImageData>(iMSTK_DATA_ROOT "/textures/fleshDiffuse.jpg");
- material->addTexture(std::make_shared<Texture>(diffuseTex, Texture::Type::Diffuse));
- auto normalTex = MeshIO::read<ImageData>(iMSTK_DATA_ROOT "/textures/fleshNormal.jpg");
- material->addTexture(std::make_shared<Texture>(normalTex, Texture::Type::Normal));
- auto ormTex = MeshIO::read<ImageData>(iMSTK_DATA_ROOT "/textures/fleshORM.jpg");
- material->addTexture(std::make_shared<Texture>(ormTex, Texture::Type::ORM));
- material->setNormalStrength(0.3);
- //material->setOpacity(0.5);
+ /* material->setShadingModel(RenderMaterial::ShadingModel::PBR);
+ auto diffuseTex = MeshIO::read<ImageData>(iMSTK_DATA_ROOT "/textures/fleshDiffuse.jpg");
+ material->addTexture(std::make_shared<Texture>(diffuseTex, Texture::Type::Diffuse));
+ auto normalTex = MeshIO::read<ImageData>(iMSTK_DATA_ROOT "/textures/fleshNormal.jpg");
+ material->addTexture(std::make_shared<Texture>(normalTex, Texture::Type::Normal));
+ auto ormTex = MeshIO::read<ImageData>(iMSTK_DATA_ROOT "/textures/fleshORM.jpg");
+ material->addTexture(std::make_shared<Texture>(ormTex, Texture::Type::ORM));
+ material->setNormalStrength(0.3);*/
+ material->setOpacity(0.5);
// Add a visual model to render the surface of the tet mesh
imstkNew<VisualModel> visualModel(surfMesh);
@@ -265,16 +282,17 @@ makeToolObj()
{
imstkNew<LineMesh> toolGeometry;
imstkNew<VecDataArray<double, 3>> verticesPtr(2);
- (*verticesPtr)[0] = Vec3d(0.0, -1.0, 0.0);
- (*verticesPtr)[1] = Vec3d(0.0, 1.0, 0.0);
+ (*verticesPtr)[0] = Vec3d(0.0, -0.05, 0.0);
+ (*verticesPtr)[1] = Vec3d(0.0, 0.05, 0.0);
imstkNew<VecDataArray<int, 2>> indicesPtr(1);
(*indicesPtr)[0] = Vec2i(0, 1);
toolGeometry->initialize(verticesPtr, indicesPtr);
auto syringeMesh = MeshIO::read<SurfaceMesh>(iMSTK_DATA_ROOT "/Surgical Instruments/Syringes/Disposable_Syringe.stl");
- syringeMesh->scale(0.4, Geometry::TransformType::ApplyToData);
syringeMesh->rotate(Vec3d(1.0, 0.0, 0.0), -PI_2, Geometry::TransformType::ApplyToData);
syringeMesh->translate(Vec3d(0.0, 4.4, 0.0), Geometry::TransformType::ApplyToData);
+ syringeMesh->scale(0.0055, Geometry::TransformType::ApplyToData);
+ syringeMesh->translate(Vec3d(0.0, 0.1, 0.0));
imstkNew<NeedleObject> toolObj("NeedleRbdTool");
toolObj->setVisualGeometry(syringeMesh);
@@ -285,16 +303,15 @@ makeToolObj()
toolObj->getVisualModel(0)->getRenderMaterial()->setShadingModel(RenderMaterial::ShadingModel::PBR);
toolObj->getVisualModel(0)->getRenderMaterial()->setRoughness(0.5);
toolObj->getVisualModel(0)->getRenderMaterial()->setMetalness(1.0);
- //toolObj->getVisualModel(0)->getRenderMaterial()->setLineWidth(5.0);
std::shared_ptr<RigidBodyModel2> rbdModel = std::make_shared<RigidBodyModel2>();
rbdModel->getConfig()->m_gravity = Vec3d::Zero();
- rbdModel->getConfig()->m_maxNumIterations = 2;
+ rbdModel->getConfig()->m_maxNumIterations = 5;
toolObj->setDynamicalModel(rbdModel);
- toolObj->getRigidBody()->m_mass = 0.5;
+ toolObj->getRigidBody()->m_mass = 1.0;
toolObj->getRigidBody()->m_intertiaTensor = Mat3d::Identity() * 10000.0;
- toolObj->getRigidBody()->m_initPos = Vec3d(0.0, 2.0, 0.0);
+ toolObj->getRigidBody()->m_initPos = Vec3d(0.0, 0.1, 0.0);
return toolObj;
}
@@ -311,21 +328,43 @@ main()
// Setup the scene
imstkNew<Scene> scene("PBDTissueVolumeNeedleContact");
- scene->getActiveCamera()->setPosition(-0.06, 7.29, 11.69);
- scene->getActiveCamera()->setFocalPoint(0.0, 0.0, 0.0);
- scene->getActiveCamera()->setViewUp(0.0, 1.0, 0.0);
+ scene->getActiveCamera()->setPosition(-0.00149496, 0.0562587, 0.168353);
+ scene->getActiveCamera()->setFocalPoint(0.00262407, -0.026582, -0.00463737);
+ scene->getActiveCamera()->setViewUp(-0.00218222, 0.901896, -0.431947);
// Setup a tissue with surface collision geometry
+ // 0.1m tissue patch 6x3x6 tet grid
std::shared_ptr<PbdObject> tissueObj = makeTissueObj("PBDTissue",
- Vec3d(10.0, 3.0, 10.0), Vec3i(7, 3, 6), Vec3d(0.1, -1.0, 0.0));
+ Vec3d(0.1, 0.025, 0.1), Vec3i(6, 3, 6), Vec3d(0.0, -0.03, 0.0));
scene->addSceneObject(tissueObj);
// Setup a tool for the user to move
std::shared_ptr<NeedleObject> toolObj = makeToolObj();
+ toolObj->setForceThreshold(15.0);
scene->addSceneObject(toolObj);
+ // Setup a debug ghost tool for virtual coupling
+ auto ghostToolObj = std::make_shared<SceneObject>("ghostTool");
+ {
+ auto toolMesh = std::dynamic_pointer_cast<SurfaceMesh>(toolObj->getVisualGeometry());
+ imstkNew<SurfaceMesh> toolGhostMesh;
+ toolGhostMesh->initialize(
+ std::make_shared<VecDataArray<double, 3>>(*toolMesh->getVertexPositions(Geometry::DataType::PreTransform)),
+ std::make_shared<VecDataArray<int, 3>>(*toolMesh->getTriangleIndices()));
+ ghostToolObj->setVisualGeometry(toolGhostMesh);
+ ghostToolObj->getVisualModel(0)->getRenderMaterial()->setColor(Color::Orange);
+ ghostToolObj->getVisualModel(0)->getRenderMaterial()->setLineWidth(5.0);
+ ghostToolObj->getVisualModel(0)->getRenderMaterial()->setOpacity(0.3);
+ }
+ scene->addSceneObject(ghostToolObj);
+
+ // Setup a debug polygon soup for debug contact points
+ imstkNew<DebugGeometryObject> debugGeomObj;
+ debugGeomObj->setLineWidth(0.1);
+ scene->addSceneObject(debugGeomObj);
+
+ // This adds both contact and puncture functionality
auto interaction = std::make_shared<NeedleInteraction>(tissueObj, toolObj);
- interaction->setForceThreshold(250.0);
scene->getCollisionGraph()->addInteraction(interaction);
// Light
@@ -351,60 +390,95 @@ main()
imstkNew<SimulationManager> driver;
driver->addModule(viewer);
driver->addModule(sceneManager);
- driver->setDesiredDt(0.001);
+ driver->setDesiredDt(0.001); // 1ms, 1000hz
#ifdef iMSTK_USE_OPENHAPTICS
imstkNew<HapticDeviceManager> hapticManager;
- hapticManager->setSleepDelay(0.1); // Delay for 1ms (haptics thread is limited to max 1000hz)
+ //hapticManager->setSleepDelay(0.01);
std::shared_ptr<HapticDeviceClient> hapticDeviceClient = hapticManager->makeDeviceClient();
driver->addModule(hapticManager);
imstkNew<RigidObjectController> controller(toolObj, hapticDeviceClient);
- controller->setTranslationScaling(0.05);
- controller->setLinearKs(1000.0);
- controller->setLinearKd(50.0);
+ controller->setTranslationScaling(0.001);
+ controller->setLinearKs(10000.0);
+ controller->setLinearKd(300.0);
controller->setAngularKs(10000000.0);
- controller->setAngularKd(500000.0);
- controller->setForceScaling(0.005);
- controller->setSmoothingKernelSize(20);
+ controller->setAngularKd(1000000.0);
+ /*controller->setAngularKs(0.0);
+ controller->setAngularKd(0.0);*/
+ controller->setForceScaling(0.08);
+ controller->setSmoothingKernelSize(5);
controller->setUseForceSmoothening(true);
scene->addController(controller);
+
+ connect<Event>(sceneManager, &SceneManager::postUpdate, [&](Event*)
+ {
+ // Update the ghost debug geometry
+ std::shared_ptr<Geometry> toolGhostMesh = ghostToolObj->getVisualGeometry();
+ toolGhostMesh->setRotation(controller->getRotation());
+ toolGhostMesh->setTranslation(controller->getPosition());
+ toolGhostMesh->updatePostTransformData();
+ toolGhostMesh->postModified();
+
+ ghostToolObj->getVisualModel(0)->getRenderMaterial()->setOpacity(std::min(1.0, controller->getForce().norm() / 15.0));
+ });
#else
- // Use keyboard controls
- connect<Event>(sceneManager, SceneManager::preUpdate, [&](Event*)
+ connect<Event>(sceneManager, &SceneManager::postUpdate, [&](Event*)
{
- if (viewer->getKeyboardDevice()->getButton('k') == KEY_PRESS)
- {
- (*toolObj->getRigidBody()->m_pos) += Vec3d(0.0, -0.01, 0.0);
- }
- else if (viewer->getKeyboardDevice()->getButton('i') == KEY_PRESS)
- {
- (*toolObj->getRigidBody()->m_pos) += Vec3d(0.0, 0.01, 0.0);
- }
- else if (viewer->getKeyboardDevice()->getButton('j') == KEY_PRESS)
- {
- (*toolObj->getRigidBody()->m_pos) += Vec3d(-0.01, 0.0, 0.0);
- }
- else if (viewer->getKeyboardDevice()->getButton('l') == KEY_PRESS)
- {
- (*toolObj->getRigidBody()->m_pos) += Vec3d(0.01, 0.0, 0.0);
- }
- else if (viewer->getKeyboardDevice()->getButton('u') == KEY_PRESS)
- {
- (*toolObj->getRigidBody()->m_pos) += Vec3d(0.0, 0.0, -0.01);
- }
- else if (viewer->getKeyboardDevice()->getButton('o') == KEY_PRESS)
+ const Vec2d mousePos = viewer->getMouseDevice()->getPos();
+ const Vec3d desiredPos = Vec3d(mousePos[0] - 0.5, mousePos[1] - 0.5, 0.0) * 0.1;
+ const Quatd desiredOrientation = Quatd(Rotd(0.0, Vec3d(1.0, 0.0, 0.0)));
+
+ Vec3d virtualForce;
{
- (*toolObj->getRigidBody()->m_pos) += Vec3d(0.0, 0.0, 0.01);
+ const Vec3d fS = (desiredPos - toolObj->getRigidBody()->getPosition()) * 1000.0; // Spring force
+ const Vec3d fD = -toolObj->getRigidBody()->getVelocity() * 100.0; // Spring damping
+
+ const Quatd dq = desiredOrientation * toolObj->getRigidBody()->getOrientation().inverse();
+ const Rotd angleAxes = Rotd(dq);
+ const Vec3d tS = angleAxes.axis() * angleAxes.angle() * 10000000.0;
+ const Vec3d tD = -toolObj->getRigidBody()->getAngularVelocity() * 1000.0;
+
+ virtualForce = fS + fD;
+ (*toolObj->getRigidBody()->m_force) += virtualForce;
+ (*toolObj->getRigidBody()->m_torque) += tS + tD;
}
- });
+
+ // Update the ghost debug geometry
+ std::shared_ptr<Geometry> toolGhostMesh = ghostToolObj->getVisualGeometry();
+ toolGhostMesh->setRotation(desiredOrientation);
+ toolGhostMesh->setTranslation(desiredPos);
+ toolGhostMesh->updatePostTransformData();
+ toolGhostMesh->postModified();
+
+ ghostToolObj->getVisualModel(0)->getRenderMaterial()->setOpacity(std::min(1.0, virtualForce.norm() / 15.0));
+ });
#endif
connect<Event>(sceneManager, &SceneManager::postUpdate, [&](Event*)
{
// Keep the tool moving in real time
toolObj->getRigidBodyModel2()->getConfig()->m_dt = sceneManager->getDt();
- });
+
+ // Copy debug geometry
+ auto needleEmbeddedCH = std::dynamic_pointer_cast<NeedleEmbeddedCH>(interaction->getEmbeddingCH());
+ const std::vector<Vec3d>& debugEmbeddingPts = needleEmbeddedCH->m_debugEmbeddingPoints;
+ const std::vector<Vec3i>& debugEmbeddingTris = needleEmbeddedCH->m_debugEmbeddedTriangles;
+ debugGeomObj->clear();
+ for (int i = 0; i < debugEmbeddingPts.size(); i++)
+ {
+ debugGeomObj->addPoint(debugEmbeddingPts[i]);
+ }
+ auto verticesPtr = std::dynamic_pointer_cast<TetrahedralMesh>(tissueObj->getPhysicsGeometry())->getVertexPositions();
+ VecDataArray<double, 3>& vertices = *verticesPtr;
+ for (int i = 0; i < debugEmbeddingTris.size(); i++)
+ {
+ debugGeomObj->addTriangle(
+ vertices[debugEmbeddingTris[i][0]],
+ vertices[debugEmbeddingTris[i][1]],
+ vertices[debugEmbeddingTris[i][2]]);
+ }
+ });
// Add mouse and keyboard controls to the viewer
{
diff --git a/Examples/PBD/PBDTissueVolumeNeedleContact/PbdTriangleEmbeddingConstraint.h b/Examples/PBD/PBDTissueVolumeNeedleContact/PbdTriangleEmbeddingConstraint.h
deleted file mode 100644
index 87c6eb4e242a747e0ca978d7d9db83eb25b301b2..0000000000000000000000000000000000000000
--- a/Examples/PBD/PBDTissueVolumeNeedleContact/PbdTriangleEmbeddingConstraint.h
+++ /dev/null
@@ -1,168 +0,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.
-
-=========================================================================*/
-
-#pragma once
-
-#include "imstkPbdCollisionConstraint.h"
-
-namespace imstk
-{
-///
-/// \class PbdTriangleEmbeddingConstraint
-///
-/// \brief Constraint to keep a specified vertex at a given uvw in a triangle
-///
-class PbdTriangleEmbeddingConstraint : public PbdCollisionConstraint
-{
-public:
- PbdTriangleEmbeddingConstraint() : PbdCollisionConstraint(1, 3) { }
- ~PbdTriangleEmbeddingConstraint() override = default;
-
-public:
- ///
- /// \brief Returns the type of the pbd collision constraint
- ///
- Type getType() const { return Type::PointTriangle; }
-
- ///
- /// \brief initialize constraint
- /// \param pIdxA1 index of the point from object1
- /// \param pIdxB1 first point of the triangle from object2
- /// \param pIdxB2 second point of the triangle from object2
- /// \param pIdxB3 third point of the triangle from object2
- /// \return
- ///
- void initConstraint(VertexMassPair ptA,
- VertexMassPair ptB1, VertexMassPair ptB2, VertexMassPair ptB3,
- double stiffnessA, double stiffnessB,
- Vec3d* p, Vec3d* q,
- double friction)
- {
- m_bodiesFirst[0] = ptA;
-
- m_bodiesSecond[0] = ptB1;
- m_bodiesSecond[1] = ptB2;
- m_bodiesSecond[2] = ptB3;
-
- m_stiffnessA = stiffnessA;
- m_stiffnessB = stiffnessB;
- m_friction = friction;
-
- const Vec3d& x0 = *m_bodiesFirst[0].vertex; // Intersection point
- const Vec3d& x1 = *m_bodiesSecond[0].vertex;
- const Vec3d& x2 = *m_bodiesSecond[1].vertex;
- const Vec3d& x3 = *m_bodiesSecond[2].vertex;
-
- // Compute the interpolant on the triangle
- {
- // Compute barycentric coordinates of ptA in tri B
- // This effectively puts the point in a local coordinate
- // system of the triangle so when the triangle begins
- // to move and deform the embedded point will as well
- const Vec3d v0 = x2 - x1;
- const Vec3d v1 = x3 - x1;
- const Vec3d v2 = x0 - x1;
- const double d00 = v0.dot(v0);
- const double d01 = v0.dot(v1);
- const double d11 = v1.dot(v1);
- const double d20 = v2.dot(v0);
- const double d21 = v2.dot(v1);
- const double denom = d00 * d11 - d01 * d01;
- const double v = (d11 * d20 - d01 * d21) / denom;
- const double w = (d00 * d21 - d01 * d20) / denom;
- const double u = 1.0 - v - w;
- m_uvw[0] = u;
- m_uvw[1] = v;
- m_uvw[2] = w;
- }
- // Compute the interpolant on the line
- {
- m_p = p;
- m_q = q;
- const Vec3d pq = (*p - *q).normalized();
- const Vec3d d = x0 - *q;
- m_t = pq.dot(d);
- }
- }
-
- // Sets the point of vertex
- VertexMassPair& getVertexMassA()
- {
- return m_bodiesFirst[0];
- }
-
- ///
- /// \brief compute value and gradient of constraint function
- ///
- /// \param[in] currVertexPositionsA current positions from object A
- /// \param[in] currVertexPositionsA current positions from object B
- /// \param[inout] c constraint value
- /// \param[inout] dcdx constraint gradient
- ///
- bool computeValueAndGradient(double& c,
- std::vector<Vec3d>& dcdxA,
- std::vector<Vec3d>& dcdxB) const override
- {
- //const Vec3d& x0 = *m_bodiesFirst[0].vertex;
- const Vec3d& x1 = *m_bodiesSecond[0].vertex;
- const Vec3d& x2 = *m_bodiesSecond[1].vertex;
- const Vec3d& x3 = *m_bodiesSecond[2].vertex;
-
- Vec3d* p = m_p;
- Vec3d* q = m_q;
- const Vec3d pq = (*p - *q);
- const Vec3d pq_n = pq.normalized();
-
- const Vec3d triPos = x1 * m_uvw[0] + x2 * m_uvw[1] + x3 * m_uvw[2];
- const Vec3d linePos = (*q) + pq_n * m_t;
-
- // Directly transform to each other
- Vec3d diff = triPos - linePos;
- // Remove normal component to allow slide (don't completely remove to allow a sort of friction)
- diff = diff - diff.dot(pq_n) * pq_n * (1.0 - m_friction);
- const Vec3d n = diff.normalized();
-
- dcdxA[0] = Vec3d::Zero();
- dcdxB[0] = n;
- dcdxB[1] = n;
- dcdxB[2] = n;
-
- c = -diff.norm();
-
- return true;
- }
-
-protected:
- // Interpolant for triangle (uv coords)
- Vec3d m_uvw = Vec3d::Zero();
-
- // Interpolant for line
- double m_t = 0.0;
-
- // Line
- Vec3d* m_p = nullptr;
- Vec3d* m_q = nullptr;
-
- bool m_enabled = true;
-
- double m_friction = 0.99;
-};
-}
\ No newline at end of file
diff --git a/Examples/PBD/PBDTissueVolumeNeedleContact/RbdLinearNeedleLockingConstraint.h b/Examples/PBD/PBDTissueVolumeNeedleContact/RbdLinearNeedleLockingConstraint.h
deleted file mode 100644
index a50b5f91c2a6b5343d573a95092fd7f4595c49a9..0000000000000000000000000000000000000000
--- a/Examples/PBD/PBDTissueVolumeNeedleContact/RbdLinearNeedleLockingConstraint.h
+++ /dev/null
@@ -1,89 +0,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.
-
-=========================================================================*/
-
-#pragma once
-
-#include "imstkRbdConstraint.h"
-
-#include <functional>
-
-using namespace imstk;
-
-///
-/// \class RbdLinearNeedleLockingConstraint
-///
-/// \brief Constrains the body to specified orientation
-/// and only allows linear movement along the inital axes
-///
-class RbdLinearNeedleLockingConstraint : public RbdConstraint
-{
-public:
- RbdLinearNeedleLockingConstraint(
- std::shared_ptr<RigidBody> obj,
- Vec3d* initNeedleAxesPt,
- Vec3d* initNeedleAxes,
- //const Quatd& initNeedleOrientation,
- const double beta = 0.05) : RbdConstraint(obj, nullptr, Side::A),
- m_initNeedleAxesPt(initNeedleAxesPt),
- m_initNeedleAxes(initNeedleAxes),
- //m_initNeedleOrientation(initNeedleOrientation),
- m_beta(beta)
- {
- }
-
- ~RbdLinearNeedleLockingConstraint() override = default;
-
-public:
- void compute(double dt) override
- {
- // Jacobian of contact (defines linear and angular constraint axes)
- J = Eigen::Matrix<double, 3, 4>::Zero();
- if ((m_side == Side::AB || m_side == Side::A) && !m_obj1->m_isStatic)
- {
- // Displacement to needle Axes
- const Vec3d diff = m_obj1->getPosition() - (*m_initNeedleAxesPt);
- const Vec3d perpDisplacement = diff - (*m_initNeedleAxes).dot(diff) * (*m_initNeedleAxes);
- const double displacement = perpDisplacement.norm();
- if (displacement != 0)
- {
- const Vec3d displacementDir = perpDisplacement / displacement;
- vu = displacement * m_beta / dt;
-
- // Displacement from center of mass
- J(0, 0) = -displacementDir[0]; J(0, 1) = 0.0;
- J(1, 0) = -displacementDir[1]; J(1, 1) = 0.0;
- J(2, 0) = -displacementDir[2]; J(2, 1) = 0.0;
-
- // Move the coupled needle location but not completely rigidly
- *m_initNeedleAxesPt += displacementDir * 0.001;
- }
- else
- {
- vu = 0.0;
- }
- }
- }
-
-private:
- Vec3d* m_initNeedleAxesPt; ///> Point on the axes to constrain too
- Vec3d* m_initNeedleAxes; //> Axes to constrain too
- double m_beta = 0.05;
-};
\ No newline at end of file
diff --git a/Examples/PBD/PBDString/CMakeLists.txt b/Examples/RBD/RbdSDFPivotNeedle/CMakeLists.txt
similarity index 78%
rename from Examples/PBD/PBDString/CMakeLists.txt
rename to Examples/RBD/RbdSDFPivotNeedle/CMakeLists.txt
index ebfc89726577b3c07b3037e56dffad62a5c853ac..38630c77be291e5d33a1df91604065c1d643e9c2 100644
--- a/Examples/PBD/PBDString/CMakeLists.txt
+++ b/Examples/RBD/RbdSDFPivotNeedle/CMakeLists.txt
@@ -16,19 +16,27 @@
#
###########################################################################
-project(Example-PBDString)
+project(Example-RbdSDFPivotNeedle)
#-----------------------------------------------------------------------------
# Create executable
#-----------------------------------------------------------------------------
-imstk_add_executable(${PROJECT_NAME} pbdStringExample.cpp)
+imstk_add_executable(${PROJECT_NAME} RbdSDFPivotNeedleExample.cpp
+ NeedleObject.h
+ NeedleInteraction.h
+ NeedleInteraction.cpp
+ NeedleRigidBodyCH.h
+ NeedleRigidBodyCH.cpp
+ RbdLineToPointTranslationConstraint.h
+ RbdLineToPointRotationConstraint.h
+ NeedleRigidBodyCH.h)
#-----------------------------------------------------------------------------
# Add the target to Examples folder
#-----------------------------------------------------------------------------
-SET_TARGET_PROPERTIES (${PROJECT_NAME} PROPERTIES FOLDER Examples/PBD)
+SET_TARGET_PROPERTIES (${PROJECT_NAME} PROPERTIES FOLDER Examples/RBD)
#-----------------------------------------------------------------------------
# Link libraries to executable
#-----------------------------------------------------------------------------
-target_link_libraries(${PROJECT_NAME} SimulationManager)
+target_link_libraries(${PROJECT_NAME} SimulationManager Filtering)
diff --git a/Examples/RBD/RbdSDFPivotNeedle/NeedleInteraction.cpp b/Examples/RBD/RbdSDFPivotNeedle/NeedleInteraction.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fba61a9482a3ee904a1de29148539605c5331c67
--- /dev/null
+++ b/Examples/RBD/RbdSDFPivotNeedle/NeedleInteraction.cpp
@@ -0,0 +1,56 @@
+/*=========================================================================
+
+ 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 "NeedleInteraction.h"
+#include "imstkCollidingObject.h"
+#include "imstkCollisionDetectionAlgorithm.h"
+#include "imstkLineMesh.h"
+#include "imstkSignedDistanceField.h"
+#include "imstkTaskGraph.h"
+#include "NeedleObject.h"
+#include "NeedleRigidBodyCH.h"
+
+using namespace imstk;
+
+NeedleInteraction::NeedleInteraction(std::shared_ptr<CollidingObject> tissueObj,
+ std::shared_ptr<NeedleObject> needleObj) : RigidObjectCollision(needleObj, tissueObj, "ImplicitGeometryToPointSetCD")
+{
+ if (std::dynamic_pointer_cast<LineMesh>(needleObj->getCollidingGeometry()) == nullptr)
+ {
+ LOG(WARNING) << "NeedleInteraction only works with LineMesh collision geometry on rigid NeedleObject";
+ }
+ if (std::dynamic_pointer_cast<ImplicitGeometry>(tissueObj->getCollidingGeometry()) == nullptr)
+ {
+ LOG(WARNING) << "NeedleInteraction only works with SDF colliding geometry on colliding tissueObj";
+ }
+
+ // First replace the handlers with our needle subclasses
+
+ // This handler consumes collision data to resolve the tool from the tissue
+ // except when the needle is inserted
+ auto needleRbdCH = std::make_shared<NeedleRigidBodyCH>();
+ needleRbdCH->setInputRigidObjectA(needleObj);
+ needleRbdCH->setInputCollidingObjectB(tissueObj);
+ needleRbdCH->setInputCollisionData(getCollisionDetection()->getCollisionData());
+ needleRbdCH->setBeta(0.001);
+ needleRbdCH->getTaskNode()->m_isCritical = true;
+ setCollisionHandlingA(needleRbdCH);
+}
\ No newline at end of file
diff --git a/Examples/RBD/RbdSDFPivotNeedle/NeedleInteraction.h b/Examples/RBD/RbdSDFPivotNeedle/NeedleInteraction.h
new file mode 100644
index 0000000000000000000000000000000000000000..8d101daffa719a8fc2916746addbbfa4cbe87540
--- /dev/null
+++ b/Examples/RBD/RbdSDFPivotNeedle/NeedleInteraction.h
@@ -0,0 +1,41 @@
+/*=========================================================================
+
+ 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 "imstkRigidObjectCollision.h"
+
+using namespace imstk;
+
+class NeedleObject;
+
+///
+/// \class NeedleInteraction
+///
+/// \brief Defines interaction between NeedleObject and PbdObject
+///
+class NeedleInteraction : public RigidObjectCollision
+{
+public:
+ NeedleInteraction(std::shared_ptr<CollidingObject> tissueObj,
+ std::shared_ptr<NeedleObject> needleObj);
+ ~NeedleInteraction() override = default;
+};
\ No newline at end of file
diff --git a/Examples/RBD/RbdSDFPivotNeedle/NeedleObject.h b/Examples/RBD/RbdSDFPivotNeedle/NeedleObject.h
new file mode 100644
index 0000000000000000000000000000000000000000..ab8fbbee7bf043c2199d69218d0c7200c9d8bb91
--- /dev/null
+++ b/Examples/RBD/RbdSDFPivotNeedle/NeedleObject.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.
+
+=========================================================================*/
+
+#pragma once
+
+#include "imstkRigidObject2.h"
+
+using namespace imstk;
+
+class NeedleObject : public RigidObject2
+{
+public:
+ enum class CollisionState
+ {
+ REMOVED,
+ TOUCHING,
+ INSERTED
+ };
+
+public:
+ NeedleObject(const std::string& name) : RigidObject2(name) { }
+ virtual ~NeedleObject() = default;
+
+ virtual const std::string getTypeName() const override { return "NeedleObject"; }
+
+public:
+ void setCollisionState(const CollisionState state) { m_collisionState = state; }
+ CollisionState getCollisionState() const { return m_collisionState; }
+
+ ///
+ /// \brief Set the force threshold for the needle
+ ///
+ void setForceThreshold(const double forceThreshold) { m_forceThreshold = forceThreshold; }
+ double getForceThreshold() const { return m_forceThreshold; }
+
+ ///
+ /// \brief Returns the current axes of the needle (tip-tail)
+ ///
+ const Vec3d getAxes() const
+ {
+ return (-getCollidingGeometry()->getRotation().col(2)).normalized();
+ }
+
+protected:
+ CollisionState m_collisionState = CollisionState::REMOVED;
+ double m_forceThreshold = 10.0;
+};
\ No newline at end of file
diff --git a/Examples/RBD/RbdSDFPivotNeedle/NeedleRigidBodyCH.cpp b/Examples/RBD/RbdSDFPivotNeedle/NeedleRigidBodyCH.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7f58cce14456ad0574796660b82de87d4f730d6d
--- /dev/null
+++ b/Examples/RBD/RbdSDFPivotNeedle/NeedleRigidBodyCH.cpp
@@ -0,0 +1,129 @@
+/*=========================================================================
+
+ 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 "NeedleRigidBodyCH.h"
+#include "imstkRbdContactConstraint.h"
+#include "imstkRigidBodyModel2.h"
+#include "NeedleObject.h"
+#include "RbdLineToPointTranslationConstraint.h"
+#include "RbdLineToPointRotationConstraint.h"
+#include "imstkLineMesh.h"
+
+using namespace imstk;
+
+void
+NeedleRigidBodyCH::handle(
+ const std::vector<CollisionElement>& elementsA,
+ const std::vector<CollisionElement>& elementsB)
+{
+ // Do it the normal way
+ RigidBodyCH::handle(elementsA, elementsB);
+
+ // If no collision, needle must be removed
+ // If using point based collision in an SDF you may want a differing unpuncturing constraint
+ auto needleObj = std::dynamic_pointer_cast<NeedleObject>(getInputObjectA());
+ NeedleObject::CollisionState state = needleObj->getCollisionState();
+ if (elementsA.size() == 0)
+ {
+ if (state == NeedleObject::CollisionState::INSERTED)
+ {
+ needleObj->setCollisionState(NeedleObject::CollisionState::REMOVED);
+ LOG(INFO) << "Unpuncture!\n";
+ }
+ else if (state == NeedleObject::CollisionState::TOUCHING)
+ {
+ needleObj->setCollisionState(NeedleObject::CollisionState::REMOVED);
+ }
+ }
+}
+
+void
+NeedleRigidBodyCH::addConstraint(
+ std::shared_ptr<RigidObject2> rbdObj,
+ const Vec3d& contactPt, const Vec3d& contactNormal,
+ const double contactDepth)
+{
+ auto needleObj = std::dynamic_pointer_cast<NeedleObject>(rbdObj);
+
+ // If the normal force exceeds threshold the needle may insert
+ if (needleObj->getCollisionState() == NeedleObject::CollisionState::REMOVED)
+ {
+ needleObj->setCollisionState(NeedleObject::CollisionState::TOUCHING);
+ }
+
+ const Vec3d n = contactNormal.normalized();
+ if (needleObj->getCollisionState() == NeedleObject::CollisionState::TOUCHING)
+ {
+ // Get all inwards force
+ const Vec3d needleAxes = needleObj->getAxes();
+ const double fN = std::max(needleAxes.dot(needleObj->getRigidBody()->getForce()), 0.0);
+
+ if (fN > needleObj->getForceThreshold())
+ {
+ LOG(INFO) << "Puncture!\n";
+ needleObj->setCollisionState(NeedleObject::CollisionState::INSERTED);
+
+ // Record the axes to constrain too
+ m_initNeedleAxes = needleAxes;
+ m_initNeedleOrientation = Quatd(needleObj->getCollidingGeometry()->getRotation());
+ m_initContactPt = contactPt;
+ }
+ }
+
+ // Only add contact normal constraint if not inserted
+ NeedleObject::CollisionState state = needleObj->getCollisionState();
+ if (state == NeedleObject::CollisionState::TOUCHING)
+ {
+ auto contactConstraint = std::make_shared<RbdContactConstraint>(
+ rbdObj->getRigidBody(), nullptr,
+ n, contactPt, contactDepth,
+ m_beta,
+ RbdConstraint::Side::A);
+ contactConstraint->compute(rbdObj->getRigidBodyModel2()->getTimeStep());
+ rbdObj->getRigidBodyModel2()->addConstraint(contactConstraint);
+ }
+ // Lock to the initial axes when the needle is inserted
+ else if (state == NeedleObject::CollisionState::INSERTED)
+ {
+ auto lineMesh = std::dynamic_pointer_cast<LineMesh>(needleObj->getPhysicsGeometry());
+ Vec3d* p = &(*lineMesh->getVertexPositions())[0];
+ Vec3d* q = &(*lineMesh->getVertexPositions())[1];
+
+ // This constraint solves for the translation to bring line p,q to m_initContactPt
+ auto translationConstraint = std::make_shared<RbdLineToPointTranslationConstraint>(
+ rbdObj->getRigidBody(), m_initContactPt, p, q, 0.1);
+ translationConstraint->compute(rbdObj->getRigidBodyModel2()->getTimeStep());
+ rbdObj->getRigidBodyModel2()->addConstraint(translationConstraint);
+
+ // Bit of a cheat, but I parameterize the inertia by depth linearly. I use a large
+ // jump after x=1 to really lock in the orientation after the needle has go so far
+ double x = contactDepth / 0.02;
+ if (x > 1.0)
+ {
+ x = 100.0;
+ }
+ rbdObj->getRigidBody()->m_intertiaTensor = Mat3d::Identity() *
+ (10000.0 + x * 10000.0);
+ rbdObj->getRigidBodyModel2()->updateMass();
+ }
+}
\ No newline at end of file
diff --git a/Examples/RBD/RbdSDFPivotNeedle/NeedleRigidBodyCH.h b/Examples/RBD/RbdSDFPivotNeedle/NeedleRigidBodyCH.h
new file mode 100644
index 0000000000000000000000000000000000000000..da37cda221b711534ff72153e4841187b23e3a07
--- /dev/null
+++ b/Examples/RBD/RbdSDFPivotNeedle/NeedleRigidBodyCH.h
@@ -0,0 +1,56 @@
+/*=========================================================================
+
+ 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 "imstkRigidBodyCH.h"
+
+using namespace imstk;
+
+class NeedleRigidBodyCH : public RigidBodyCH
+{
+public:
+ NeedleRigidBodyCH() = default;
+ ~NeedleRigidBodyCH() override = default;
+
+ virtual const std::string getTypeName() const override { return "NeedleRigidBodyCH"; }
+
+protected:
+ ///
+ /// \brief Handle the collision/contact data
+ ///
+ virtual void handle(
+ const std::vector<CollisionElement>& elementsA,
+ const std::vector<CollisionElement>& elementsB) override;
+
+ ///
+ /// \brief Add constraint for the rigid body given contact
+ ///
+ void addConstraint(
+ std::shared_ptr<RigidObject2> rbdObj,
+ const Vec3d& contactPt, const Vec3d& contactNormal,
+ const double contactDepth) override;
+
+protected:
+ Vec3d m_initContactPt = Vec3d::Zero();
+ Vec3d m_initNeedleAxes = Vec3d::Zero();
+ Quatd m_initNeedleOrientation = Quatd::Identity();
+};
\ No newline at end of file
diff --git a/Examples/PBD/PBDTissueSurfaceNeedleContact/RbdAngularNeedleLockingConstraint.h b/Examples/RBD/RbdSDFPivotNeedle/RbdLineToPointRotationConstraint.h
similarity index 59%
rename from Examples/PBD/PBDTissueSurfaceNeedleContact/RbdAngularNeedleLockingConstraint.h
rename to Examples/RBD/RbdSDFPivotNeedle/RbdLineToPointRotationConstraint.h
index 1b8eb1b0625a9841f0692b1ed5bab0408036f737..1fdf9c1118e4bea381879ede0d237206be28435e 100644
--- a/Examples/PBD/PBDTissueSurfaceNeedleContact/RbdAngularNeedleLockingConstraint.h
+++ b/Examples/RBD/RbdSDFPivotNeedle/RbdLineToPointRotationConstraint.h
@@ -26,23 +26,24 @@
using namespace imstk;
///
-/// \class RbdAngularNeedleLockingConstraint
+/// \class RbdAxesLockingConstraint
///
-/// \brief Constrains the orientation to some initial orientation
+/// \brief Constraints the line p, q to the fixedPt by rotating p and q
///
-class RbdAngularNeedleLockingConstraint : public RbdConstraint
+class RbdLineToPointRotationConstraint : public RbdConstraint
{
public:
- RbdAngularNeedleLockingConstraint(
+ RbdLineToPointRotationConstraint(
std::shared_ptr<RigidBody> obj,
- const Quatd& initNeedleOrientation,
- const double beta = 0.05) : RbdConstraint(obj, nullptr, Side::A),
- m_initNeedleOrientation(initNeedleOrientation),
+ const Vec3d& fixedPt,
+ Vec3d* p, Vec3d* q,
+ const double beta = 0.05) : RbdConstraint(obj, nullptr, Side::A),
+ m_fixedPt(fixedPt), m_p(p), m_q(q),
m_beta(beta)
{
}
- ~RbdAngularNeedleLockingConstraint() override = default;
+ ~RbdLineToPointRotationConstraint() override = default;
public:
void compute(double dt) override
@@ -51,10 +52,14 @@ public:
J = Eigen::Matrix<double, 3, 4>::Zero();
if ((m_side == Side::AB || m_side == Side::A) && !m_obj1->m_isStatic)
{
- const Quatd dq = m_initNeedleOrientation * m_obj1->getOrientation().inverse();
- const Rotd angleAxes = Rotd(dq);
- const Vec3d rotAxes = angleAxes.axis();
- vu = angleAxes.angle() * m_beta / dt;
+ // Gives the rotation to bring the line p,q to pass through point fixedPt
+ const Vec3d axes = (*m_q - m_obj1->getPosition()).normalized();
+ const Vec3d diff = m_fixedPt - m_obj1->getPosition();
+ const Vec3d rot = axes.cross(diff.normalized());
+ const Vec3d rotAxes = rot.normalized();
+
+ // rot.norm gives area of crossed vectors, should be 0 when rotated to each other
+ vu = rot.norm() * m_beta / dt;
J(0, 0) = 0.0; J(0, 1) = rotAxes[0];
J(1, 0) = 0.0; J(1, 1) = rotAxes[1];
J(2, 0) = 0.0; J(2, 1) = rotAxes[2];
@@ -62,6 +67,9 @@ public:
}
private:
- Quatd m_initNeedleOrientation; ///> Orientation to fix too
double m_beta = 0.05;
+
+ Vec3d m_fixedPt;
+ Vec3d* m_p = nullptr;
+ Vec3d* m_q = nullptr;
};
\ No newline at end of file
diff --git a/Examples/RBD/RbdSDFPivotNeedle/RbdLineToPointTranslationConstraint.h b/Examples/RBD/RbdSDFPivotNeedle/RbdLineToPointTranslationConstraint.h
new file mode 100644
index 0000000000000000000000000000000000000000..39061ef0a40c0a741157c49bbebfc3b175e7d9c2
--- /dev/null
+++ b/Examples/RBD/RbdSDFPivotNeedle/RbdLineToPointTranslationConstraint.h
@@ -0,0 +1,74 @@
+/*=========================================================================
+
+ 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 "imstkRbdConstraint.h"
+
+using namespace imstk;
+
+///
+/// \class RbdLineToPointTranslationConstraint
+///
+/// \brief Constraints the line p, q to the fixedPt by rotating p and q
+///
+class RbdLineToPointTranslationConstraint : public RbdConstraint
+{
+public:
+ RbdLineToPointTranslationConstraint(
+ std::shared_ptr<RigidBody> obj,
+ const Vec3d& fixedPt,
+ Vec3d* p, Vec3d* q,
+ const double beta = 0.05) : RbdConstraint(obj, nullptr, Side::A),
+ m_fixedPt(fixedPt), m_p(p), m_q(q),
+ m_beta(beta)
+ {
+ }
+
+ ~RbdLineToPointTranslationConstraint() override = default;
+
+public:
+ void compute(double dt) override
+ {
+ // Jacobian of contact (defines linear and angular constraint axes)
+ J = Eigen::Matrix<double, 3, 4>::Zero();
+ if ((m_side == Side::AB || m_side == Side::A) && !m_obj1->m_isStatic)
+ {
+ // Gives the translation to bring the line p,q to pass through point fixedPt
+ const Vec3d axes = (*m_q - *m_p).normalized();
+ const Vec3d diff = m_fixedPt - *m_p;
+ const Vec3d vecToLine = (diff - diff.dot(axes) * axes);
+ const Vec3d dirToLine = vecToLine.normalized();
+
+ vu = vecToLine.norm() * m_beta / dt;
+ J(0, 0) = dirToLine[0]; J(0, 1) = 0.0;
+ J(1, 0) = dirToLine[1]; J(1, 1) = 0.0;
+ J(2, 0) = dirToLine[2]; J(2, 1) = 0.0;
+ }
+ }
+
+private:
+ double m_beta = 0.05;
+
+ Vec3d m_fixedPt;
+ Vec3d* m_p = nullptr;
+ Vec3d* m_q = nullptr;
+};
\ No newline at end of file
diff --git a/Examples/RBD/RbdSDFPivotNeedle/RbdSDFPivotNeedleExample.cpp b/Examples/RBD/RbdSDFPivotNeedle/RbdSDFPivotNeedleExample.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c7ccad428e618f9facd4fa2213ae8ff4310667a4
--- /dev/null
+++ b/Examples/RBD/RbdSDFPivotNeedle/RbdSDFPivotNeedleExample.cpp
@@ -0,0 +1,274 @@
+/*=========================================================================
+
+ 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 "imstkCollisionGraph.h"
+#include "imstkDirectionalLight.h"
+#include "imstkGeometryUtilities.h"
+#include "imstkIsometricMap.h"
+#include "imstkKeyboardSceneControl.h"
+#include "imstkLineMesh.h"
+#include "imstkMeshIO.h"
+#include "imstkMouseSceneControl.h"
+#include "imstkNew.h"
+#include "imstkPlane.h"
+#include "imstkRbdConstraint.h"
+#include "imstkRenderMaterial.h"
+#include "imstkRigidBodyModel2.h"
+#include "imstkScene.h"
+#include "imstkSceneManager.h"
+#include "imstkSimulationManager.h"
+#include "imstkSurfaceMesh.h"
+#include "imstkVisualModel.h"
+#include "imstkVTKViewer.h"
+#include "NeedleInteraction.h"
+#include "NeedleObject.h"
+
+#ifdef iMSTK_USE_OPENHAPTICS
+#include "imstkHapticDeviceClient.h"
+#include "imstkHapticDeviceManager.h"
+#include "imstkRigidObjectController.h"
+#else
+#include "imstkMouseDeviceClient.h"
+#endif
+
+using namespace imstk;
+
+///
+/// \brief Returns a colliding object tissue plane that uses an implicit geometry for collision
+///
+static std::shared_ptr<CollidingObject>
+createTissueObj()
+{
+ imstkNew<CollidingObject> tissueObj("Tissue");
+
+ imstkNew<Plane> tissuePlane(Vec3d(0.0, 0.0, 0.0), Vec3d(0.0, 1.0, 0.0));
+ tissuePlane->setWidth(0.1);
+
+ tissueObj->setVisualGeometry(tissuePlane);
+ tissueObj->setCollidingGeometry(tissuePlane);
+
+ auto material = std::make_shared<RenderMaterial>();
+ material->setShadingModel(RenderMaterial::ShadingModel::PBR);
+ material->setColor(Color::Bone);
+ material->setRoughness(0.5);
+ material->setMetalness(0.1);
+ tissueObj->getVisualModel(0)->setRenderMaterial(material);
+
+ return tissueObj;
+}
+
+static std::shared_ptr<NeedleObject>
+createNeedleObj()
+{
+ imstkNew<LineMesh> toolGeometry;
+ imstkNew<VecDataArray<double, 3>> verticesPtr(2);
+ (*verticesPtr)[0] = Vec3d(0.0, 0.0, 0.0);
+ (*verticesPtr)[1] = Vec3d(0.0, 0.0, -0.1);
+ imstkNew<VecDataArray<int, 2>> indicesPtr(1);
+ (*indicesPtr)[0] = Vec2i(0, 1);
+ toolGeometry->initialize(verticesPtr, indicesPtr);
+
+ auto syringeMesh = MeshIO::read<SurfaceMesh>(iMSTK_DATA_ROOT "/Surgical Instruments/Syringes/Disposable_Syringe.stl");
+ syringeMesh->scale(0.0075, Geometry::TransformType::ApplyToData);
+ toolGeometry->rotate(Vec3d(0.0, 1.0, 0.0), PI, Geometry::TransformType::ApplyToData);
+ syringeMesh->translate(Vec3d(0.0, 0.0, 0.1), Geometry::TransformType::ApplyToData);
+
+ imstkNew<NeedleObject> toolObj("NeedleRbdTool");
+ toolObj->setVisualGeometry(syringeMesh);
+ toolObj->setCollidingGeometry(toolGeometry);
+ toolObj->setPhysicsGeometry(toolGeometry);
+ toolObj->setPhysicsToVisualMap(std::make_shared<IsometricMap>(toolGeometry, syringeMesh));
+ toolObj->getVisualModel(0)->getRenderMaterial()->setColor(Color(0.9, 0.9, 0.9));
+ toolObj->getVisualModel(0)->getRenderMaterial()->setShadingModel(RenderMaterial::ShadingModel::PBR);
+ toolObj->getVisualModel(0)->getRenderMaterial()->setRoughness(0.5);
+ toolObj->getVisualModel(0)->getRenderMaterial()->setMetalness(1.0);
+ toolObj->getVisualModel(0)->getRenderMaterial()->setOpacity(0.5);
+
+ auto lineModel = std::make_shared<VisualModel>();
+ lineModel->setGeometry(toolGeometry);
+ toolObj->addVisualModel(lineModel);
+ //toolObj->getVisualModel(0)->getRenderMaterial()->setLineWidth(5.0);
+
+ std::shared_ptr<RigidBodyModel2> rbdModel = std::make_shared<RigidBodyModel2>();
+ rbdModel->getConfig()->m_gravity = Vec3d::Zero();
+ rbdModel->getConfig()->m_maxNumIterations = 20;
+ rbdModel->getConfig()->m_angularVelocityDamping = 0.8; // Helps with lack of 6dof
+ toolObj->setDynamicalModel(rbdModel);
+
+ toolObj->getRigidBody()->m_mass = 1.0;
+ toolObj->getRigidBody()->m_intertiaTensor = Mat3d::Identity() * 1000.0;
+ toolObj->getRigidBody()->m_initPos = Vec3d(0.0, 0.1, 0.0);
+
+ return toolObj;
+}
+
+///
+/// \brief This examples demonstrates rigid body with pivot constraint
+///
+int
+main()
+{
+ // Write log to stdout and file
+ Logger::startLogger();
+
+ imstkNew<Scene> scene("RbdSDFNeedle");
+
+ // Create the bone
+ std::shared_ptr<CollidingObject> tissueObj = createTissueObj();
+ scene->addSceneObject(tissueObj);
+
+ // Create the needle
+ std::shared_ptr<NeedleObject> needleObj = createNeedleObj();
+ needleObj->setForceThreshold(50.0);
+ scene->addSceneObject(needleObj);
+
+ // Setup a debug ghost tool for virtual coupling
+ auto ghostToolObj = std::make_shared<SceneObject>("ghostTool");
+ {
+ auto toolMesh = std::dynamic_pointer_cast<SurfaceMesh>(needleObj->getVisualGeometry());
+ imstkNew<SurfaceMesh> toolGhostMesh;
+ toolGhostMesh->initialize(
+ std::make_shared<VecDataArray<double, 3>>(*toolMesh->getVertexPositions(Geometry::DataType::PreTransform)),
+ std::make_shared<VecDataArray<int, 3>>(*toolMesh->getTriangleIndices()));
+ ghostToolObj->setVisualGeometry(toolGhostMesh);
+ ghostToolObj->getVisualModel(0)->getRenderMaterial()->setColor(Color::Orange);
+ ghostToolObj->getVisualModel(0)->getRenderMaterial()->setLineWidth(5.0);
+ ghostToolObj->getVisualModel(0)->getRenderMaterial()->setOpacity(0.3);
+ }
+ scene->addSceneObject(ghostToolObj);
+
+ // Setup interaction between tissue and needle
+ auto needleInteraction = std::make_shared<NeedleInteraction>(tissueObj, needleObj);
+ scene->getCollisionGraph()->addInteraction(needleInteraction);
+
+ // Camera
+ scene->getActiveCamera()->setPosition(0.0, 0.2, 0.2);
+ scene->getActiveCamera()->setFocalPoint(0.0, 0.0, 0.0);
+ scene->getActiveCamera()->setViewUp(0.0, 1.0, 0.0);
+
+ // Light
+ imstkNew<DirectionalLight> light;
+ light->setDirection(Vec3d(0.0, -1.0, -1.0));
+ light->setIntensity(1.0);
+ scene->addLight("light", light);
+
+ // Run the simulation
+ {
+ // Setup a viewer to render in its own thread
+ imstkNew<VTKViewer> viewer("Viewer");
+ viewer->setActiveScene(scene);
+ viewer->setDebugAxesLength(0.005, 0.005, 0.005);
+
+ // Setup a scene manager to advance the scene in its own thread
+ imstkNew<SceneManager> sceneManager("Scene Manager");
+ sceneManager->setActiveScene(scene);
+ sceneManager->setExecutionType(Module::ExecutionType::ADAPTIVE);
+ sceneManager->pause();
+
+ imstkNew<SimulationManager> driver;
+ driver->addModule(viewer);
+ driver->addModule(sceneManager);
+ driver->setDesiredDt(0.001);
+
+#ifdef iMSTK_USE_OPENHAPTICS
+ imstkNew<HapticDeviceManager> hapticManager;
+ std::shared_ptr<HapticDeviceClient> deviceClient = hapticManager->makeDeviceClient();
+ driver->addModule(hapticManager);
+
+ imstkNew<RigidObjectController> controller(needleObj, deviceClient);
+ controller->setTranslationScaling(0.001);
+ controller->setLinearKs(8000.0);
+ controller->setLinearKd(200.0);
+ controller->setAngularKs(1000000.0);
+ controller->setAngularKd(100000.0);
+ controller->setForceScaling(0.02);
+ controller->setSmoothingKernelSize(5);
+ controller->setUseForceSmoothening(true);
+ scene->addController(controller);
+
+ connect<Event>(sceneManager, &SceneManager::postUpdate, [&](Event*)
+ {
+ // Keep the tool moving in real time
+ needleObj->getRigidBodyModel2()->getConfig()->m_dt = sceneManager->getDt();
+
+ // Update the ghost debug geometry
+ std::shared_ptr<Geometry> toolGhostMesh = ghostToolObj->getVisualGeometry();
+ toolGhostMesh->setRotation(controller->getRotation());
+ toolGhostMesh->setTranslation(controller->getPosition());
+ toolGhostMesh->updatePostTransformData();
+ toolGhostMesh->postModified();
+
+ ghostToolObj->getVisualModel(0)->getRenderMaterial()->setOpacity(std::min(1.0, controller->getForce().norm() / 15.0));
+ });
+#else
+ connect<Event>(sceneManager, &SceneManager::postUpdate, [&](Event*)
+ {
+ // Keep the tool moving in real time
+ needleObj->getRigidBodyModel2()->getConfig()->m_dt = sceneManager->getDt();
+
+ const Vec2d mousePos = viewer->getMouseDevice()->getPos();
+ const Vec3d desiredPos = Vec3d(mousePos[0] - 0.5, mousePos[1] - 0.5, 0.0) * 0.25;
+ const Quatd desiredOrientation = Quatd(Rotd(-1.0, Vec3d(1.0, 0.0, 0.0)));
+
+ Vec3d virutalForce;
+ {
+ const Vec3d fS = (desiredPos - needleObj->getRigidBody()->getPosition()) * 1000.0; // Spring force
+ const Vec3d fD = -needleObj->getRigidBody()->getVelocity() * 100.0; // Spring damping
+
+ const Quatd dq = desiredOrientation * needleObj->getRigidBody()->getOrientation().inverse();
+ const Rotd angleAxes = Rotd(dq);
+ const Vec3d tS = angleAxes.axis() * angleAxes.angle() * 10000000.0;
+ const Vec3d tD = -needleObj->getRigidBody()->getAngularVelocity() * 1000.0;
+
+ virutalForce = fS + fD;
+ (*needleObj->getRigidBody()->m_force) += virutalForce;
+ (*needleObj->getRigidBody()->m_torque) += tS + tD;
+ }
+
+ // Update the ghost debug geometry
+ std::shared_ptr<Geometry> toolGhostMesh = ghostToolObj->getVisualGeometry();
+ toolGhostMesh->setRotation(desiredOrientation);
+ toolGhostMesh->setTranslation(desiredPos);
+ toolGhostMesh->updatePostTransformData();
+ toolGhostMesh->postModified();
+
+ ghostToolObj->getVisualModel(0)->getRenderMaterial()->setOpacity(std::min(1.0, virutalForce.norm() / 15.0));
+ });
+#endif
+
+ // 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->setModuleDriver(driver);
+ viewer->addControl(keyControl);
+ }
+
+ driver->start();
+ }
+
+ return 0;
+}
diff --git a/Examples/RBD/RbdSurfaceMeshToSphereCD/RbdSurfaceMeshToSphereCDExample.cpp b/Examples/RBD/RbdSurfaceMeshToSphereCD/RbdSurfaceMeshToSphereCDExample.cpp
index 5a41e4286467bdda3f9b112fdfa3d433b86d5fd9..ebe92231b884466890c7feb0bd674ab3120ceda2 100644
--- a/Examples/RBD/RbdSurfaceMeshToSphereCD/RbdSurfaceMeshToSphereCDExample.cpp
+++ b/Examples/RBD/RbdSurfaceMeshToSphereCD/RbdSurfaceMeshToSphereCDExample.cpp
@@ -104,7 +104,6 @@ main()
Logger::startLogger();
imstkNew<Scene> scene("RbdMeshMeshCollision");
- scene->getConfig()->taskParallelizationEnabled = false;
// This model is shared among interacting rigid bodies
imstkNew<RigidBodyModel2> rbdModel;
diff --git a/Source/Common/imstkColor.cpp b/Source/Common/imstkColor.cpp
index 63c2b31dd83d23458477b45bb113391ce365cf52..d8f73adf6aa021987688d912f2f61ca58c3aef8a 100644
--- a/Source/Common/imstkColor.cpp
+++ b/Source/Common/imstkColor.cpp
@@ -47,6 +47,8 @@ Color Color::Marigold(0.9, 0.9, 0.4);
Color Color::YellowBone(0.828, 0.785, 0.501);
Color Color::Bone(0.89, 0.86, 0.79);
Color Color::Blood(0.4, 0.0, 0.0);
+Color Color::LightSkin(1.0, 0.86, 0.7);
+Color Color::DarkSkin(0.38, 0.27, 0.18);
Color::Color()
{
diff --git a/Source/Common/imstkColor.h b/Source/Common/imstkColor.h
index 4f33c02e3dfbfbff774b59069f3b8d3452a7ebbc..c0526bbc66ce90a1e2f353afa5cf2649225c28c1 100644
--- a/Source/Common/imstkColor.h
+++ b/Source/Common/imstkColor.h
@@ -143,6 +143,8 @@ struct Color
static Color Bone;
static Color YellowBone;
static Color Blood;
+ static Color LightSkin;
+ static Color DarkSkin;
};
#ifdef WIN32
#pragma warning(default : 4201)