diff --git a/Data/laptool/lower.obj.mtl.sha512 b/Data/laptool/lower.obj.mtl.sha512
new file mode 100644
index 0000000000000000000000000000000000000000..f53743cea18b9bb1fa9bba198334ed8236babfa8
--- /dev/null
+++ b/Data/laptool/lower.obj.mtl.sha512
@@ -0,0 +1 @@
+c194f28a2e4a48a6a01439f0d5a7b3f4073887829529820e9c82232b83a7ab66962419a37f0cb7ae4b5673e094e8f39a5c1a99645f32c7c84bf1286a4df3dbf9
\ No newline at end of file
diff --git a/Data/laptool/lower.obj.sha512 b/Data/laptool/lower.obj.sha512
index 194f9d90f390cca5313de12dfe58621339b42ba6..d49b8d8a8868f497885576eea1f816769109d67e 100644
--- a/Data/laptool/lower.obj.sha512
+++ b/Data/laptool/lower.obj.sha512
@@ -1 +1 @@
-eab8e05fbbfa17a05b680429142d27e7021f16b90927aba8b04341718a4e56344f2e113cc3861fb77ff74774c3f99e3db03a636b3c6bc6447fdcc352e0986f0a
\ No newline at end of file
+c5d915dc6eb1aa7907aba0bf0dd558a551017ae46aa3651d523ba614248a32c100427030da890549f9397e8941cf5ee8cd39b0fa9300d71c5bf3fb7ec4f22440
\ No newline at end of file
diff --git a/Data/laptool/pivot.obj.mtl.sha512 b/Data/laptool/pivot.obj.mtl.sha512
new file mode 100644
index 0000000000000000000000000000000000000000..1252f24ed286ecb48afdbbbe6c71b53f13b44d10
--- /dev/null
+++ b/Data/laptool/pivot.obj.mtl.sha512
@@ -0,0 +1 @@
+4c1ce8e1fb894be9a295b22824f999c8cd7099d59c1e8239dd025cb8dda8fc53da4a60351dd1d25aa32101365f4557944d0664e96d7ea81b8e9f9794568101ee
\ No newline at end of file
diff --git a/Data/laptool/pivot.obj.sha512 b/Data/laptool/pivot.obj.sha512
index bd4cbe61d1f983b6fbb3cd17a47f684f0e19860e..87ccd3521d06408ecd4c8e9d61018b0e05eb4994 100644
--- a/Data/laptool/pivot.obj.sha512
+++ b/Data/laptool/pivot.obj.sha512
@@ -1 +1 @@
-3f0838c3cd99192ed22004aaab9090f023513be0f0c3af086a3a5ab487b40ba9da7e319269a4cf2c36b905b840adff47cbf19c5d403c63414d68779a60c0e7f8
\ No newline at end of file
+8655ed1b030b9101aaf7543727de275312e2e29f7aa2310ae1565169fe0ef49801eb87bfe617132324e03fe65e99f8b7c54518a196575c87d96d3babfc649ffe
\ No newline at end of file
diff --git a/Data/laptool/upper.obj.mtl.sha512 b/Data/laptool/upper.obj.mtl.sha512
new file mode 100644
index 0000000000000000000000000000000000000000..f49550458c875ed079ef754a46664ac0d5b60d0e
--- /dev/null
+++ b/Data/laptool/upper.obj.mtl.sha512
@@ -0,0 +1 @@
+9585d6e768cf02902b53afc772e212a7d111d5c37724db55ca316cd44ab1bae5670bd5f12ed76516dc88ab02638e2fba507d8a2d169e39af8ca96841c7885925
\ No newline at end of file
diff --git a/Data/laptool/upper.obj.sha512 b/Data/laptool/upper.obj.sha512
index 5bf3c309f5312dcf74d8d671997a3d2bd1b061f7..631fbad50008f7cb27000fc8b5047731e3458e3a 100644
--- a/Data/laptool/upper.obj.sha512
+++ b/Data/laptool/upper.obj.sha512
@@ -1 +1 @@
-6b55f6d9ec3d16a6385faffb181c5579bcb7acc6405953599abd603d45b72153542175436ba2a3f02ee8fbb2d1b122538d91a8a00d3284b4e94e0fb429c95385
\ No newline at end of file
+9947ea15f111d68f11bdd51d06019cb12932a10d0ee4c2e8d1410e7733d6938d483d6373e258c6fbcee60df3daf0cdab68ef5415b905bcc94df7d06a5bd88a86
\ No newline at end of file
diff --git a/Examples/PBDPicking/CMakeLists.txt b/Examples/PBDPicking/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..17355b81fce860db0699f5899fb289f6b03935e5
--- /dev/null
+++ b/Examples/PBDPicking/CMakeLists.txt
@@ -0,0 +1,37 @@
+###########################################################################
+#
+# 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.
+#
+###########################################################################
+if(iMSTK_USE_OpenHaptics)
+
+ project(Example-PBDPicking)
+
+ #-----------------------------------------------------------------------------
+ # Create executable
+ #-----------------------------------------------------------------------------
+ imstk_add_executable(${PROJECT_NAME} PBDPickingExample.cpp)
+
+ #-----------------------------------------------------------------------------
+ # Add the target to Examples folder
+ #-----------------------------------------------------------------------------
+ SET_TARGET_PROPERTIES (${PROJECT_NAME} PROPERTIES FOLDER Examples/Haptics)
+
+ #-----------------------------------------------------------------------------
+ # Link libraries to executable
+ #-----------------------------------------------------------------------------
+ target_link_libraries(${PROJECT_NAME} SimulationManager apiUtilities)
+
+endif()
diff --git a/Examples/PBDPicking/PBDPickingExample.cpp b/Examples/PBDPicking/PBDPickingExample.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bdb0ae14c008db7e261d37a5baf7acee193a3ce0
--- /dev/null
+++ b/Examples/PBDPicking/PBDPickingExample.cpp
@@ -0,0 +1,302 @@
+/*=========================================================================
+
+ 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 "imstkLogger.h"
+#include "imstkNew.h"
+#include "imstkScene.h"
+#include "imstkSceneManager.h"
+#include "imstkVTKViewer.h"
+
+// Objects
+#include "imstkCamera.h"
+#include "imstkCollidingObject.h"
+#include "imstkLight.h"
+#include "imstkPbdObject.h"
+
+// Geometry
+#include "imstkMeshIO.h"
+#include "imstkCapsule.h"
+#include "imstkSurfaceMesh.h"
+
+// Devices and controllers
+#include "imstkHapticDeviceClient.h"
+#include "imstkHapticDeviceManager.h"
+#include "imstkKeyboardDeviceClient.h"
+#include "imstkKeyboardSceneControl.h"
+#include "imstkLaparoscopicToolController.h"
+#include "imstkMouseSceneControl.h"
+
+// Collisions and Models
+#include "imstkCollisionGraph.h"
+#include "imstkCollisionPair.h"
+#include "imstkCollisionDetection.h"
+#include "imstkPbdModel.h"
+#include "imstkPbdObjectPickingPair.h"
+#include "imstkPBDPickingCH.h"
+#include "imstkRenderMaterial.h"
+#include "imstkVisualModel.h"
+
+// global variables
+const std::string phantomOmni1Name = "Default Device";
+
+using namespace imstk;
+
+// Parameters to play with
+const double width = 50.0;
+const double height = 50.0;
+const int nRows = 16;
+const int nCols = 16;
+
+///
+/// \brief Creates cloth geometry
+///
+static std::shared_ptr<SurfaceMesh>
+makeClothGeometry(const double width,
+ const double height,
+ const int nRows,
+ const int nCols)
+{
+ imstkNew<SurfaceMesh> clothMesh;
+
+ StdVectorOfVec3d vertList;
+ vertList.resize(nRows * nCols);
+ const double dy = width / (double)(nCols - 1);
+ const double dx = height / (double)(nRows - 1);
+ for (int i = 0; i < nRows; ++i)
+ {
+ for (int j = 0; j < nCols; j++)
+ {
+ vertList[i * nCols + j] = Vec3d((double)dx * i, 1.0, (double)dy * j);
+ }
+ }
+ clothMesh->setInitialVertexPositions(vertList);
+ clothMesh->setVertexPositions(vertList);
+
+ // Add connectivity data
+ std::vector<SurfaceMesh::TriangleArray> triangles;
+ for (std::size_t i = 0; i < nRows - 1; ++i)
+ {
+ for (std::size_t j = 0; j < nCols - 1; j++)
+ {
+ SurfaceMesh::TriangleArray tri[2];
+ const size_t index1 = i * nCols + j;
+ const size_t index2 = index1 + nCols;
+ const size_t index3 = index1 + 1;
+ const size_t index4 = index2 + 1;
+
+ // Interleave [/][\]
+ if (i % 2 ^ j % 2)
+ {
+ tri[0] = { { index1, index2, index3 } };
+ tri[1] = { { index4, index3, index2 } };
+ }
+ else
+ {
+ tri[0] = { { index2, index4, index1 } };
+ tri[1] = { { index4, index3, index1 } };
+ }
+ triangles.push_back(tri[0]);
+ triangles.push_back(tri[1]);
+ }
+ }
+
+ clothMesh->setTrianglesVertices(triangles);
+
+ return clothMesh;
+}
+
+///
+/// \brief Creates cloth object
+///
+static std::shared_ptr<PbdObject>
+makeClothObj(const std::string& name,
+ const double width,
+ const double height,
+ const int nRows,
+ const int nCols)
+{
+ imstkNew<PbdObject> clothObj(name);
+
+ // Setup the Geometry
+ std::shared_ptr<SurfaceMesh> clothMesh(makeClothGeometry(width, height, nRows, nCols));
+
+ // Setup the Parameters
+ imstkNew<PBDModelConfig> pbdParams;
+ pbdParams->enableConstraint(PbdConstraint::Type::Distance, 1.0e2);
+ pbdParams->enableConstraint(PbdConstraint::Type::Dihedral, 1.0e1);
+ pbdParams->m_fixedNodeIds = { 0, static_cast<size_t>(nCols) - 1 };
+ pbdParams->m_uniformMassValue = width * height / ((double)nRows * (double)nCols);
+ pbdParams->m_gravity = Vec3d(0.0, -9.8, 0.0);
+ pbdParams->m_defaultDt = 0.005;
+ pbdParams->m_iterations = 5;
+
+ // Setup the Model
+ imstkNew<PbdModel> pbdModel;
+ pbdModel->setModelGeometry(clothMesh);
+ pbdModel->configure(pbdParams);
+
+ // Setup the VisualModel
+ imstkNew<RenderMaterial> material;
+ material->setBackFaceCulling(false);
+ material->setDisplayMode(RenderMaterial::DisplayMode::WireframeSurface);
+
+ imstkNew<VisualModel> visualModel(clothMesh);
+ visualModel->setRenderMaterial(material);
+
+ // Setup the Object
+ clothObj->addVisualModel(visualModel);
+ clothObj->setPhysicsGeometry(clothMesh);
+ clothObj->setCollidingGeometry(clothMesh);
+ clothObj->setDynamicalModel(pbdModel);
+
+ return clothObj;
+}
+
+///
+/// \brief This example demonstrates the concept of PBD picking
+/// for haptic interaction. NOTE: Requires GeoMagic Touch device
+///
+int
+main()
+{
+ // Setup logger (write to file and stdout)
+ Logger::startLogger();
+
+ // Scene
+ imstkNew<Scene> scene("PBDPicking");
+
+ // Create the virtual coupling object controller
+
+ // Device Server
+ imstkNew<HapticDeviceManager> server;
+ std::shared_ptr<HapticDeviceClient> client = server->makeDeviceClient(phantomOmni1Name);
+
+ // Load the meshes
+ auto upperSurfMesh = MeshIO::read<SurfaceMesh>(iMSTK_DATA_ROOT "/laptool/upper.obj");
+ auto lowerSurfMesh = MeshIO::read<SurfaceMesh>(iMSTK_DATA_ROOT "/laptool/lower.obj");
+ auto pivotSurfMesh = MeshIO::read<SurfaceMesh>(iMSTK_DATA_ROOT "/laptool/pivot.obj");
+
+ imstkNew<Capsule> geomShaft;
+ geomShaft->setLength(20.0);
+ geomShaft->setRadius(1.0);
+ geomShaft->setOrientationAxis(Vec3d(0.0, 0.0, 1.0));
+ geomShaft->setTranslation(Vec3d(0.0, 0.0, 10.0));
+ imstkNew<CollidingObject> objShaft("ShaftObject");
+ objShaft->setVisualGeometry(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(1.0);
+ geomUpperJaw->setOrientationAxis(Vec3d(0.0, 0.0, 1.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(1.0);
+ geomLowerJaw->setOrientationAxis(Vec3d(0.0, 0.0, 1.0));
+ imstkNew<CollidingObject> objLowerJaw("LowerJawObject");
+ objLowerJaw->setVisualGeometry(lowerSurfMesh);
+ objLowerJaw->setCollidingGeometry(geomLowerJaw);
+ scene->addSceneObject(objLowerJaw);
+
+ std::shared_ptr<PbdObject> clothObj = makeClothObj("Cloth", width, height, nRows, nCols);
+ scene->addSceneObject(clothObj);
+
+ // Create and add virtual coupling object controller in the scene
+ imstkNew<LaparoscopicToolController> controller(objShaft, objUpperJaw, objLowerJaw, client);
+ controller->setJawAngleChange(6.0e-3);
+ scene->addController(controller);
+
+ // Add interaction pair for pbd picking
+ imstkNew<PbdObjectPickingPair> upperJawPickingPair(clothObj, objUpperJaw, CollisionDetection::Type::PointSetToCapsule);
+ imstkNew<PbdObjectPickingPair> lowerJawPickingPair(clothObj, objLowerJaw, CollisionDetection::Type::PointSetToCapsule);
+ scene->getCollisionGraph()->addInteraction(upperJawPickingPair);
+ scene->getCollisionGraph()->addInteraction(lowerJawPickingPair);
+
+ // Camera
+ scene->getActiveCamera()->setPosition(Vec3d(1, 1, 1) * 100.0);
+ scene->getActiveCamera()->setFocalPoint(Vec3d(0, -50, 0));
+
+ // Light
+ imstkNew<DirectionalLight> light("light");
+ light->setFocalPoint(Vec3d(5.0, -8.0, -5.0));
+ light->setIntensity(1.0);
+ scene->addLight(light);
+
+ //Run the simulation
+ {
+ // Setup a viewer to render in its own thread
+ imstkNew<VTKViewer> viewer("Viewer");
+ viewer->setActiveScene(scene);
+
+ // Setup a scene manager to advance the scene in its own thread
+ imstkNew<SceneManager> sceneManager("Scene Manager");
+ sceneManager->setActiveScene(scene);
+ viewer->addChildThread(sceneManager); // SceneManager will start/stop with viewer
+
+ viewer->addChildThread(server);
+
+ // Add mouse and keyboard controls to the viewer
+ {
+ imstkNew<MouseSceneControl> mouseControl(viewer->getMouseDevice());
+ mouseControl->setSceneManager(sceneManager);
+ viewer->addControl(mouseControl);
+
+ imstkNew<KeyboardSceneControl> keyControl(viewer->getKeyboardDevice());
+ keyControl->setSceneManager(sceneManager);
+ keyControl->setViewer(viewer);
+ viewer->addControl(keyControl);
+ }
+ // Not perfectly thread safe movement lambda, ijkl movement instead of wasd because d is already used
+ std::shared_ptr<KeyboardDeviceClient> keyDevice = viewer->getKeyboardDevice();
+ connect<Event>(sceneManager, EventType::PreUpdate, [&](Event*)
+ {
+ std::shared_ptr<PBDPickingCH> chUpper = std::static_pointer_cast<PBDPickingCH>(upperJawPickingPair->getCollisionHandlingA());
+ std::shared_ptr<PBDPickingCH> chLower = std::static_pointer_cast<PBDPickingCH>(lowerJawPickingPair->getCollisionHandlingA());
+
+ // Activate picking
+ if (client->getButton(1))
+ {
+ chUpper->activatePickConstraints();
+ chLower->activatePickConstraints();
+ }
+ // Unpick
+ if (client->getButton(0))
+ {
+ chUpper->removePickConstraints();
+ chLower->removePickConstraints();
+ }
+ });
+
+ // Start viewer running, scene as paused
+ sceneManager->requestStatus(ThreadStatus::Paused);
+ viewer->start();
+ }
+
+ return 0;
+}
diff --git a/Source/CollisionDetection/CollisionDetection/imstkNarrowPhaseCD.cpp b/Source/CollisionDetection/CollisionDetection/imstkNarrowPhaseCD.cpp
index d6fe6dcea7feaf8d93ba91602ea39a5cc0f0ddd7..9ef14b60192eb2b3933a8fbe8e865699ff1b0882 100644
--- a/Source/CollisionDetection/CollisionDetection/imstkNarrowPhaseCD.cpp
+++ b/Source/CollisionDetection/CollisionDetection/imstkNarrowPhaseCD.cpp
@@ -254,9 +254,9 @@ pointToCapsule(const Vec3r& point, uint32_t pointIdx, Capsule* const capsule,
// Get position of end points of the capsule
// TODO: Fix this issue of extra computation in future
- const Vec3d p0 = capsulePos;
- const Vec3d p1 = p0 + capsule->getOrientationAxis() * length;
- const Vec3d mid = 0.5 * (p0 + p1);
+ const Vec3d mid = capsulePos;
+ const Vec3d p1 = mid + 0.5 * capsule->getOrientationAxis() * length;
+ const Vec3d p0 = 2 * mid - p1;
const Vec3d p = p1 - p0;
const auto pDotp = p.dot(p);
const auto pDotp0 = p.dot(p0);
diff --git a/Source/CollisionHandling/imstkCollisionHandling.h b/Source/CollisionHandling/imstkCollisionHandling.h
index ec367199aa9ba16ed09e25de9e508a37a0084887..46ffed1cd6f0190cd4e5e9a5a5a4726f8b435c19 100644
--- a/Source/CollisionHandling/imstkCollisionHandling.h
+++ b/Source/CollisionHandling/imstkCollisionHandling.h
@@ -51,6 +51,7 @@ public:
BoneDrilling,
SPH,
PBD,
+ PBDPicking,
RBD,
LevelSet
};
diff --git a/Source/CollisionHandling/imstkPBDPickingCH.cpp b/Source/CollisionHandling/imstkPBDPickingCH.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6191eb4d02752a4ca81362cad28652146de2a473
--- /dev/null
+++ b/Source/CollisionHandling/imstkPBDPickingCH.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.
+
+=========================================================================*/
+
+#include "imstkAnalyticalGeometry.h"
+#include "imstkCollisionData.h"
+#include "imstkPbdModel.h"
+#include "imstkPbdObject.h"
+#include "imstkPBDPickingCH.h"
+#include "imstkPointSet.h"
+
+namespace imstk
+{
+PBDPickingCH::PBDPickingCH(const CollisionHandling::Side& side,
+ const std::shared_ptr<CollisionData> colData,
+ std::shared_ptr<PbdObject> pbdObj,
+ std::shared_ptr<CollidingObject> pickObj) :
+ CollisionHandling(Type::PBDPicking, side, colData),
+ m_pbdObj(pbdObj),
+ m_pickObj(pickObj)
+{
+ m_isPicking = false;
+ m_pickedPtIdxOffset.clear();
+}
+
+void
+PBDPickingCH::processCollisionData()
+{
+ CHECK(m_pbdObj != nullptr && m_pickObj != nullptr)
+ << "PBDPickingCH::handleCollision error: "
+ << "no picking collision handling available the object";
+
+ if (m_isPicking)
+ {
+ this->updatePickConstraints();
+ }
+}
+
+void
+PBDPickingCH::updatePickConstraints()
+{
+ if (m_pickedPtIdxOffset.size() == 0)
+ {
+ this->removePickConstraints();
+ return;
+ }
+
+ std::shared_ptr<PbdModel> model = m_pbdObj->getPbdModel();
+ std::shared_ptr<AnalyticalGeometry> pickGeom = std::dynamic_pointer_cast<AnalyticalGeometry>(m_pickObj->getCollidingGeometry());
+ for (auto iter = m_pickedPtIdxOffset.begin(); iter != m_pickedPtIdxOffset.end(); iter++)
+ {
+ auto rot = pickGeom->getRotation();
+ model->getCurrentState()->setVertexPosition(iter->first, pickGeom->getPosition() + rot * iter->second);
+ }
+}
+
+void
+PBDPickingCH::addPickConstraints(std::shared_ptr<PbdObject> pbdObj, std::shared_ptr<CollidingObject> pickObj)
+{
+ if (m_colData->MAColData.isEmpty())
+ {
+ return;
+ }
+
+ CHECK(pbdObj != nullptr && pickObj != nullptr)
+ << "PBDPickingCH:addPickConstraints error: "
+ << "no pdb object or colliding object.";
+
+ std::shared_ptr<PbdModel> model = pbdObj->getPbdModel();
+ std::shared_ptr<AnalyticalGeometry> pickGeom = std::dynamic_pointer_cast<AnalyticalGeometry>(pickObj->getCollidingGeometry());
+ CHECK(pickGeom != nullptr) << "Colliding geometry is analytical geometry ";
+
+ ParallelUtils::SpinLock lock;
+ ParallelUtils::parallelFor(m_colData->MAColData.getSize(),
+ [&](const size_t idx) {
+ const auto& cd = m_colData->MAColData[idx];
+ if (m_pickedPtIdxOffset.find(cd.nodeIdx) == m_pickedPtIdxOffset.end() && model->getInvMass(cd.nodeIdx) != 0.0)
+ {
+ auto rot = pickGeom->getRotation().transpose();
+ auto relativePos = rot * (model->getCurrentState()->getVertexPosition(cd.nodeIdx) - pickGeom->getPosition());
+
+ lock.lock();
+ m_pickedPtIdxOffset[cd.nodeIdx] = relativePos;
+ model->setFixedPoint(cd.nodeIdx);
+ model->getCurrentState()->setVertexPosition(cd.nodeIdx, pickGeom->getPosition() + rot.transpose() * relativePos);
+ lock.unlock();
+ }
+ });
+}
+
+void
+PBDPickingCH::removePickConstraints()
+{
+ std::shared_ptr<PbdModel> model = m_pbdObj->getPbdModel();
+ m_isPicking = false;
+ for (auto iter = m_pickedPtIdxOffset.begin(); iter != m_pickedPtIdxOffset.end(); ++iter)
+ {
+ model->setPointUnfixed(iter->first);
+ }
+ m_pickedPtIdxOffset.clear();
+}
+
+void
+PBDPickingCH::activatePickConstraints()
+{
+ if (!m_colData->MAColData.isEmpty())
+ {
+ this->addPickConstraints(m_pbdObj, m_pickObj);
+ m_isPicking = true;
+ }
+}
+}
diff --git a/Source/CollisionHandling/imstkPBDPickingCH.h b/Source/CollisionHandling/imstkPBDPickingCH.h
new file mode 100644
index 0000000000000000000000000000000000000000..e8bd4ab38153abbc2d9aa49e7ac2caae21fbd1e9
--- /dev/null
+++ b/Source/CollisionHandling/imstkPBDPickingCH.h
@@ -0,0 +1,88 @@
+/*=========================================================================
+
+ 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 "imstkCollisionHandling.h"
+
+#include "imstkMath.h"
+namespace imstk
+{
+class CollidingObject;
+class PbdObject;
+struct CollisionData;
+
+///
+/// \class PBDPickingCH
+///
+/// \brief Implements nodal picking for PBD object
+///
+class PBDPickingCH : public CollisionHandling
+{
+public:
+
+ ///
+ /// \brief Constructor
+ ///
+ PBDPickingCH(const Side& side,
+ const std::shared_ptr<CollisionData> colData,
+ std::shared_ptr<PbdObject> pbdObj,
+ std::shared_ptr<CollidingObject> pickObj);
+
+ PBDPickingCH() = delete;
+
+ ///
+ /// \brief Destructor
+ ///
+ virtual ~PBDPickingCH() override = default;
+
+ ///
+ /// \brief Compute forces based on collision data
+ ///
+ void processCollisionData() override;
+
+ ///
+ /// \brief Add picking constraints for the node that is picked
+ ///
+ void addPickConstraints(std::shared_ptr<PbdObject> pbdObj, std::shared_ptr<CollidingObject> pickObj);
+
+ ///
+ /// \brief Update picking constraints for the node that is picked
+ ///
+ void updatePickConstraints();
+
+ ///
+ /// \brief Remove picking constraints for the node that is picked
+ ///
+ void removePickConstraints();
+
+ ///
+ /// \brief Activate picking constraints for nodes in the collision data
+ ///
+ void activatePickConstraints();
+
+private:
+ bool m_isPicking;
+ std::map<size_t, Vec3d> m_pickedPtIdxOffset; ///> Map for picked nodes.
+ std::shared_ptr<PbdObject> m_pbdObj; ///> PBD object
+ std::shared_ptr<CollidingObject> m_pickObj; ///> Picking tool object
+};
+}
diff --git a/Source/Common/imstkMath.h b/Source/Common/imstkMath.h
index fed99309ed902ad6667757fc1ef7919b27b7f07a..e94c5585fdadc5ff58e36515ba55f50f9d1a1f56 100644
--- a/Source/Common/imstkMath.h
+++ b/Source/Common/imstkMath.h
@@ -166,4 +166,67 @@ using AffineTransform3d = Eigen::Affine3d;
#define MAX_F std::numeric_limits<float>::max()
#define MIN_F std::numeric_limits<float>::min()
#define VERY_SMALL_EPSILON_F std::numeric_limits<float>::epsilon()
+
+static inline Mat4d
+mat4dTranslate(const Vec3d& translate)
+{
+ return AffineTransform3d(Eigen::Translation3d(translate)).matrix();
+}
+
+static inline Mat4d
+mat4dScale(const Vec3d& scale)
+{
+ Mat4d mScale = Mat4d::Identity();
+ mScale(0, 0) = scale[0];
+ mScale(1, 1) = scale[1];
+ mScale(2, 2) = scale[2];
+ return mScale;
+}
+
+static inline Mat4d
+mat4dRotation(const Quatd& rotation)
+{
+ Mat4d mRot = Mat4d::Identity();
+ mRot.block<3, 3>(0, 0) = rotation.toRotationMatrix();
+ return mRot;
+}
+
+static inline Mat4d
+mat4dRotation(const Rotd& rotation)
+{
+ Mat4d mRot = Mat4d::Identity();
+ mRot.block<3, 3>(0, 0) = rotation.toRotationMatrix();
+ return mRot;
+}
+
+static inline Mat4d
+mat4dRotation(const Mat3d& rotation)
+{
+ Mat4d mRot = Mat4d::Identity();
+ mRot.block<3, 3>(0, 0) = rotation;
+ return mRot;
+}
+
+///
+/// \brief Translation, Rotation, Scaling decomposition, ignores shears
+///
+static inline void
+mat4dTRS(const Mat4d& m, Vec3d& t, Mat3d& r, Vec3d& s)
+{
+ // Assumes affine, no shear
+ const Vec3d& x = m.block<3, 1>(0, 0);
+ const Vec3d& y = m.block<3, 1>(0, 1);
+ const Vec3d& z = m.block<3, 1>(0, 2);
+
+ s = Vec3d(m.block<3, 1>(0, 0).norm(),
+ m.block<3, 1>(0, 1).norm(),
+ m.block<3, 1>(0, 2).norm());
+
+ r = Mat3d::Identity();
+ r.block<3, 1>(0, 0) = x.normalized();
+ r.block<3, 1>(0, 1) = y.normalized();
+ r.block<3, 1>(0, 2) = z.normalized();
+
+ t = m.block<3, 1>(0, 3);
+}
}
diff --git a/Source/Controllers/imstkLaparoscopicToolController.cpp b/Source/Controllers/imstkLaparoscopicToolController.cpp
index 1aacbc3cb9bddaacedaff70dbfdc90976e6e9fe5..c1369d87533870b7931586163d744b3c59bc495c 100644
--- a/Source/Controllers/imstkLaparoscopicToolController.cpp
+++ b/Source/Controllers/imstkLaparoscopicToolController.cpp
@@ -1,111 +1,149 @@
-/*=========================================================================
-
- Library: iMSTK
-
- Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
- & Imaging in Medicine, Rensselaer Polytechnic Institute.
-
- Licensed under the Apache License, Version 2.0 (the "License");
- you may not use this file except in compliance with the License.
- You may obtain a copy of the License at
-
- http://www.apache.org/licenses/LICENSE-2.0.txt
-
- Unless required by applicable law or agreed to in writing, software
- distributed under the License is distributed on an "AS IS" BASIS,
- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- See the License for the specific language governing permissions and
- limitations under the License.
-
-=========================================================================*/
-
-#include "imstkLaparoscopicToolController.h"
-#include "imstkCollidingObject.h"
-#include "imstkDeviceClient.h"
-#include "imstkGeometry.h"
-#include "imstkLogger.h"
-
-namespace imstk
-{
-LaparoscopicToolController::LaparoscopicToolController(
- std::shared_ptr<SceneObject> shaft,
- std::shared_ptr<SceneObject> upperJaw,
- std::shared_ptr<SceneObject> lowerJaw,
- std::shared_ptr<DeviceClient> trackingDevice) :
- TrackingDeviceControl(trackingDevice),
- m_shaft(shaft),
- m_upperJaw(upperJaw),
- m_lowerJaw(lowerJaw),
- m_jawRotationAxis(Vec3d(0, 1., 0))
-{
- trackingDevice->setButtonsEnabled(true);
-}
-
-void
-LaparoscopicToolController::updateControlledObjects()
-{
- if (!isTrackerUpToDate())
- {
- if (!updateTrackingData())
- {
- LOG(WARNING) << "LaparoscopicToolController::updateControlledObjects warning: could not update tracking info.";
- return;
- }
- }
-
- Vec3d p = getPosition();
- Quatd r = getRotation();
-
- // Update jaw angles
- if (m_deviceClient->getButton(0))
- {
- m_jawAngle += m_change;
- m_jawAngle = (m_jawAngle > m_maxJawAngle) ? m_maxJawAngle : m_jawAngle;
- }
-
- if (m_deviceClient->getButton(1))
- {
- m_jawAngle -= m_change;
- m_jawAngle = (m_jawAngle < 0.0) ? 0.0 : m_jawAngle;
- }
-
- // Update orientation of parts
- Quatd jawRotUpper;
- jawRotUpper = r * Rotd(m_jawAngle, m_jawRotationAxis);
- m_upperJaw->getMasterGeometry()->setRotation(jawRotUpper);
-
- Quatd jawRotLower;
- jawRotLower = r * Rotd(-m_jawAngle, m_jawRotationAxis);
- m_lowerJaw->getMasterGeometry()->setRotation(jawRotLower);
-
- m_shaft->getMasterGeometry()->setRotation(r);
-
- // Update positions of parts
- m_shaft->getMasterGeometry()->setTranslation(p);
- m_upperJaw->getMasterGeometry()->setTranslation(p);
- m_lowerJaw->getMasterGeometry()->setTranslation(p);
-}
-
-void
-LaparoscopicToolController::applyForces()
-{
- Vec3d force(0, 0, 0);
-
- if (auto collidingObject = dynamic_cast<CollidingObject*>(m_shaft.get()))
- {
- force += collidingObject->getForce();
- }
-
- if (auto collidingObject = dynamic_cast<CollidingObject*>(m_upperJaw.get()))
- {
- force += collidingObject->getForce();
- }
-
- if (auto collidingObject = dynamic_cast<CollidingObject*>(m_lowerJaw.get()))
- {
- force += collidingObject->getForce();
- }
-
- m_deviceClient->setForce(force);
-}
-} // imstk
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0.txt
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+
+=========================================================================*/
+
+#include "imstkLaparoscopicToolController.h"
+#include "imstkCollidingObject.h"
+#include "imstkDeviceClient.h"
+#include "imstkGeometry.h"
+#include "imstkLogger.h"
+
+namespace imstk
+{
+LaparoscopicToolController::LaparoscopicToolController(
+ std::shared_ptr<CollidingObject> shaft,
+ std::shared_ptr<CollidingObject> upperJaw,
+ std::shared_ptr<CollidingObject> lowerJaw,
+ std::shared_ptr<DeviceClient> trackingDevice) :
+ TrackingDeviceControl(trackingDevice),
+ m_shaft(shaft),
+ m_upperJaw(upperJaw),
+ m_lowerJaw(lowerJaw),
+ m_jawRotationAxis(Vec3d(1, 0, 0))
+{
+ trackingDevice->setButtonsEnabled(true);
+
+ // Record the transforms as 4x4 matrices (this should capture initial displacement/rotation of the jaws/shaft from controller)
+ m_shaftVisualTransform = mat4dTranslate(m_shaft->getVisualGeometry()->getTranslation()) * mat4dRotation(m_shaft->getVisualGeometry()->getRotation());
+ m_upperJawVisualTransform = mat4dTranslate(m_upperJaw->getVisualGeometry()->getTranslation()) * mat4dRotation(m_upperJaw->getVisualGeometry()->getRotation());
+ m_lowerJawVisualTransform = mat4dTranslate(m_lowerJaw->getVisualGeometry()->getTranslation()) * mat4dRotation(m_lowerJaw->getVisualGeometry()->getRotation());
+
+ m_shaftCollidingTransform = mat4dTranslate(m_shaft->getCollidingGeometry()->getTranslation()) * mat4dRotation(m_shaft->getCollidingGeometry()->getRotation());
+ m_upperJawCollidingTransform = mat4dTranslate(m_upperJaw->getCollidingGeometry()->getTranslation()) * mat4dRotation(m_upperJaw->getCollidingGeometry()->getRotation());
+ m_lowerJawCollidingTransform = mat4dTranslate(m_lowerJaw->getCollidingGeometry()->getTranslation()) * mat4dRotation(m_lowerJaw->getCollidingGeometry()->getRotation());
+}
+
+void
+LaparoscopicToolController::updateControlledObjects()
+{
+ if (!isTrackerUpToDate())
+ {
+ if (!updateTrackingData())
+ {
+ LOG(WARNING) << "LaparoscopicToolController::updateControlledObjects warning: could not update tracking info.";
+ return;
+ }
+ }
+
+ const Vec3d controllerPosition = getPosition();
+ const Quatd controllerOrientation = getRotation();
+
+ // Controller transform
+ m_controllerWorldTransform = mat4dTranslate(controllerPosition) * mat4dRotation(controllerOrientation);
+
+ // TRS decompose and set shaft geometries
+ {
+ Vec3d t, s;
+ Mat3d r;
+
+ mat4dTRS(m_controllerWorldTransform * m_shaftVisualTransform, t, r, s);
+ m_shaft->getVisualGeometry()->setRotation(r);
+ m_shaft->getVisualGeometry()->setTranslation(t);
+
+ mat4dTRS(m_controllerWorldTransform * m_shaftCollidingTransform, t, r, s);
+ m_shaft->getCollidingGeometry()->setRotation(r);
+ m_shaft->getCollidingGeometry()->setTranslation(t);
+ }
+
+ // Update jaw angles
+ if (m_deviceClient->getButton(0))
+ {
+ m_jawAngle += m_change;
+ m_jawAngle = (m_jawAngle > m_maxJawAngle) ? m_maxJawAngle : m_jawAngle;
+ }
+ if (m_deviceClient->getButton(1))
+ {
+ m_jawAngle -= m_change;
+ m_jawAngle = (m_jawAngle < 0.0) ? 0.0 : m_jawAngle;
+ }
+
+ m_upperJawLocalTransform = mat4dRotation(Rotd(m_jawAngle, m_jawRotationAxis));
+ m_lowerJawLocalTransform = mat4dRotation(Rotd(-m_jawAngle, m_jawRotationAxis));
+
+ // TRS decompose and set upper/lower jaw geometries
+ {
+ Vec3d t, s;
+ Mat3d r;
+
+ mat4dTRS(m_controllerWorldTransform * m_upperJawLocalTransform * m_upperJawVisualTransform, t, r, s);
+ m_upperJaw->getVisualGeometry()->setRotation(r);
+ m_upperJaw->getVisualGeometry()->setTranslation(t);
+
+ mat4dTRS(m_controllerWorldTransform * m_upperJawLocalTransform * m_upperJawCollidingTransform, t, r, s);
+ m_upperJaw->getCollidingGeometry()->setRotation(r);
+ m_upperJaw->getCollidingGeometry()->setTranslation(t);
+ }
+ {
+ Vec3d t, s;
+ Mat3d r;
+
+ mat4dTRS(m_controllerWorldTransform * m_lowerJawLocalTransform * m_upperJawVisualTransform, t, r, s);
+ m_lowerJaw->getVisualGeometry()->setRotation(r);
+ m_lowerJaw->getVisualGeometry()->setTranslation(t);
+
+ mat4dTRS(m_controllerWorldTransform * m_lowerJawLocalTransform * m_lowerJawCollidingTransform, t, r, s);
+ m_lowerJaw->getCollidingGeometry()->setRotation(r);
+ m_lowerJaw->getCollidingGeometry()->setTranslation(t);
+ }
+}
+
+void
+LaparoscopicToolController::applyForces()
+{
+ Vec3d force(0, 0, 0);
+
+ if (auto collidingObject = dynamic_cast<CollidingObject*>(m_shaft.get()))
+ {
+ force += collidingObject->getForce();
+ }
+
+ if (auto collidingObject = dynamic_cast<CollidingObject*>(m_upperJaw.get()))
+ {
+ force += collidingObject->getForce();
+ }
+
+ if (auto collidingObject = dynamic_cast<CollidingObject*>(m_lowerJaw.get()))
+ {
+ force += collidingObject->getForce();
+ }
+
+ m_deviceClient->setForce(force);
+}
+} // imstk
diff --git a/Source/Controllers/imstkLaparoscopicToolController.h b/Source/Controllers/imstkLaparoscopicToolController.h
index 2c1e1c9c41d504478ec2cafe31863fbe9de52a16..29657ff2da0f379c03b707d61a5521f16f909c1b 100644
--- a/Source/Controllers/imstkLaparoscopicToolController.h
+++ b/Source/Controllers/imstkLaparoscopicToolController.h
@@ -1,101 +1,114 @@
-/*=========================================================================
-
- Library: iMSTK
-
- Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
- & Imaging in Medicine, Rensselaer Polytechnic Institute.
-
- Licensed under the Apache License, Version 2.0 (the "License");
- you may not use this file except in compliance with the License.
- You may obtain a copy of the License at
-
- http://www.apache.org/licenses/LICENSE-2.0.txt
-
- Unless required by applicable law or agreed to in writing, software
- distributed under the License is distributed on an "AS IS" BASIS,
- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- See the License for the specific language governing permissions and
- limitations under the License.
-
-=========================================================================*/
-
-#pragma once
-
-#include "imstkTrackingDeviceControl.h"
-
-namespace imstk
-{
-class SceneObject;
-
-///
-/// \class LaparoscopicToolController
-///
-/// \brief Two-jawed laparoscopic tool controlled by external device
-/// The tool is composed of three scene objects: pivot, lower jaw and upper jaw
-/// The jaws open-close based on the buttons at present.
-/// This has to be replaced by potentiometer tracking in future.
-///
-class LaparoscopicToolController : public TrackingDeviceControl
-{
-public:
- ///
- /// \brief Constructor
- ///
- LaparoscopicToolController(
- std::shared_ptr<SceneObject> shaft,
- std::shared_ptr<SceneObject> upperJaw,
- std::shared_ptr<SceneObject> lowerJaw,
- std::shared_ptr<DeviceClient> trackingDevice);
-
- LaparoscopicToolController() = delete; //not allowed for now
-
- ~LaparoscopicToolController() override = default;
-
-public:
- ///
- /// \brief Update controlled laparoscopic tool using latest tracking information
- ///
- void updateControlledObjects() override;
-
- ///
- /// \brief Apply forces to the haptic device
- ///
- void applyForces() override;
-
- ///
- /// \brief Set the maximum jaw angle
- ///
- inline void setMaxJawAngle(const double maxAngle) { m_maxJawAngle = maxAngle; }
-
- ///
- /// \brief Set the increment
- ///
- inline void setJawAngleChange(const double dAngle) { m_change = dAngle; }
-
- ///
- /// \brief Set the jaw rotation axis
- ///
- inline void setJawRotationAxis(const Vec3d& axis) { m_jawRotationAxis = axis; }
-
- ///
- /// \brief Get the current jaw angle
- ///
- inline double getJawAngle() const { return m_jawAngle; }
-
- ///
- /// \brief Get the max jaw angle
- ///
- inline double getMaxJawAngle() const { return m_maxJawAngle; }
-
-protected:
- std::shared_ptr<SceneObject> m_shaft; ///< Tool shaft
- std::shared_ptr<SceneObject> m_upperJaw; ///< Tool upper jaw
- std::shared_ptr<SceneObject> m_lowerJaw; ///< Tool lower jaw
-
- double m_jawAngle = PI / 6.0; ///< Angle of the jaws
- double m_change = 6.0e-5; ///< Amount of change in jaw angle per frame
- double m_maxJawAngle = PI / 6.0; ///< Maximum angle of the jaws
-
- Vec3d m_jawRotationAxis; ///< Angle of the jaws
-};
-} // imstk
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0.txt
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+
+=========================================================================*/
+
+#pragma once
+
+#include "imstkTrackingDeviceControl.h"
+
+namespace imstk
+{
+class CollidingObject;
+
+///
+/// \class LaparoscopicToolController
+///
+/// \brief Two-jawed laparoscopic tool controlled by external device
+/// The tool is composed of three scene objects: pivot, lower jaw and upper jaw
+/// The jaws open-close based on the buttons at present.
+/// This has to be replaced by potentiometer tracking in future.
+///
+class LaparoscopicToolController : public TrackingDeviceControl
+{
+public:
+ ///
+ /// \brief Constructor
+ ///
+ LaparoscopicToolController(
+ std::shared_ptr<CollidingObject> shaft,
+ std::shared_ptr<CollidingObject> upperJaw,
+ std::shared_ptr<CollidingObject> lowerJaw,
+ std::shared_ptr<DeviceClient> trackingDevice);
+
+ LaparoscopicToolController() = delete; //not allowed for now
+
+ ~LaparoscopicToolController() override = default;
+
+public:
+ ///
+ /// \brief Update controlled laparoscopic tool using latest tracking information
+ ///
+ void updateControlledObjects() override;
+
+ ///
+ /// \brief Apply forces to the haptic device
+ ///
+ void applyForces() override;
+
+ ///
+ /// \brief Set the maximum jaw angle
+ ///
+ inline void setMaxJawAngle(const double maxAngle) { m_maxJawAngle = maxAngle; }
+
+ ///
+ /// \brief Set the increment
+ ///
+ inline void setJawAngleChange(const double dAngle) { m_change = dAngle; }
+
+ ///
+ /// \brief Set the jaw rotation axis
+ ///
+ inline void setJawRotationAxis(const Vec3d& axis) { m_jawRotationAxis = axis; }
+
+ ///
+ /// \brief Get the current jaw angle
+ ///
+ inline double getJawAngle() const { return m_jawAngle; }
+
+ ///
+ /// \brief Get the max jaw angle
+ ///
+ inline double getMaxJawAngle() const { return m_maxJawAngle; }
+
+protected:
+ std::shared_ptr<CollidingObject> m_shaft; ///< Tool shaft
+ std::shared_ptr<CollidingObject> m_upperJaw; ///< Tool upper jaw
+ std::shared_ptr<CollidingObject> m_lowerJaw; ///< Tool lower jaw
+
+ double m_jawAngle = PI / 6.0; ///< Angle of the jaws
+ double m_change = 6.0e-5; ///< Amount of change in jaw angle per frame
+ double m_maxJawAngle = PI / 6.0; ///< Maximum angle of the jaws
+
+ Vec3d m_jawRotationAxis; ///< Angle of the jaws
+
+ Mat4d m_controllerWorldTransform = Mat4d::Identity(); // Final world transform of the controller
+
+ Mat4d m_shaftVisualTransform = Mat4d::Identity(); // Initial local transform of the visual shaft
+ Mat4d m_upperJawVisualTransform = Mat4d::Identity(); // Initial local transform of the visual upper jaw
+ Mat4d m_lowerJawVisualTransform = Mat4d::Identity(); // Initial local transform of the visual lower jaw
+
+ Mat4d m_shaftCollidingTransform = Mat4d::Identity(); // Initial local transform of the colliding shaft
+ Mat4d m_upperJawCollidingTransform = Mat4d::Identity(); // Initial local transform of the colliding upper jaw
+ Mat4d m_lowerJawCollidingTransform = Mat4d::Identity(); // Initial local transform of the colliding lower jaw
+
+ Mat4d m_upperJawLocalTransform = Mat4d::Identity(); // upperJawWorldTransform = m_controllerWorldTransform * m_upperJawLocalTransform * m_upperJawVisual/CollidingTransform
+ Mat4d m_lowerJawLocalTransform = Mat4d::Identity(); // lowerJawWorldTransform = m_controllerWorldTransform * m_lowerJawLocalTransform * m_lowerJawVisual/CollidingTransform
+};
+} // imstk
diff --git a/Source/DynamicalModels/ObjectModels/imstkPbdModel.cpp b/Source/DynamicalModels/ObjectModels/imstkPbdModel.cpp
index e95368d560fe689b565016ae2c1a86631c27a3c9..59e930ed8a700dd773742630873643e1da6f870d 100644
--- a/Source/DynamicalModels/ObjectModels/imstkPbdModel.cpp
+++ b/Source/DynamicalModels/ObjectModels/imstkPbdModel.cpp
@@ -41,6 +41,7 @@ namespace imstk
PbdModel::PbdModel() : DynamicalModel(DynamicalModelType::PositionBasedDynamics),
m_mass(std::make_shared<StdVectorOfReal>()),
m_invMass(std::make_shared<StdVectorOfReal>()),
+ m_fixedNodeInvMass(std::make_shared<std::map<size_t, double>>()),
m_constraints(std::make_shared<PBDConstraintVector>()),
m_partitionedConstraints(std::make_shared<std::vector<PBDConstraintVector>>()),
m_parameters(std::make_shared<PBDModelConfig>())
@@ -663,13 +664,27 @@ PbdModel::setParticleMass(const double val, const size_t idx)
void
PbdModel::setFixedPoint(const size_t idx)
{
- StdVectorOfReal& invMasses = *m_invMass;
+ StdVectorOfReal& invMasses = *m_invMass;
+ std::map<size_t, double>& fixedNodeInvMass = *m_fixedNodeInvMass;
if (idx < m_mesh->getNumVertices())
{
+ fixedNodeInvMass[idx] = invMasses[idx];
invMasses[idx] = 0.0;
}
}
+void
+PbdModel::setPointUnfixed(const size_t idx)
+{
+ StdVectorOfReal& invMasses = *m_invMass;
+ std::map<size_t, double>& fixedNodeInvMass = *m_fixedNodeInvMass;
+ if (fixedNodeInvMass.find(idx) != fixedNodeInvMass.end())
+ {
+ invMasses[idx] = fixedNodeInvMass[idx];
+ fixedNodeInvMass.erase(idx);
+ }
+}
+
void
PbdModel::integratePosition()
{
diff --git a/Source/DynamicalModels/ObjectModels/imstkPbdModel.h b/Source/DynamicalModels/ObjectModels/imstkPbdModel.h
index 641797c75ed9dcb577a1acda8f5a3a78caaf068e..730db658f64f9553b46038c33c0e0664645e2741 100644
--- a/Source/DynamicalModels/ObjectModels/imstkPbdModel.h
+++ b/Source/DynamicalModels/ObjectModels/imstkPbdModel.h
@@ -26,6 +26,8 @@
#include "imstkPbdFEMConstraint.h"
#include "imstkPbdState.h"
+#include <map>
+
namespace imstk
{
class PointSet;
@@ -209,10 +211,15 @@ public:
void setParticleMass(const double val, const size_t idx);
///
- /// \brief Se the node as fixed
+ /// \brief Set the node as fixed
///
void setFixedPoint(const size_t idx);
+ ///
+ /// \brief Set the node as unfixed
+ ///
+ void setPointUnfixed(const size_t idx);
+
///
/// \brief Get the inverse of mass of a certain node
///
@@ -277,6 +284,7 @@ protected:
std::shared_ptr<PointSet> m_mesh = nullptr; ///> PointSet on which the pbd model operates on
std::shared_ptr<StdVectorOfReal> m_mass = nullptr; ///> Mass of nodes
std::shared_ptr<StdVectorOfReal> m_invMass = nullptr; ///> Inverse of mass of nodes
+ std::shared_ptr<std::map<size_t, double>> m_fixedNodeInvMass = nullptr; ///> Map for archiving fixed nodes' mass.
std::shared_ptr<PBDConstraintVector> m_constraints = nullptr; ///> List of pbd constraints
std::shared_ptr<std::vector<PBDConstraintVector>> m_partitionedConstraints = nullptr; ///> List of pbd constraints
diff --git a/Source/Geometry/Analytic/imstkCapsule.cpp b/Source/Geometry/Analytic/imstkCapsule.cpp
index 266fd3e508dd614e0739e28a37db74df1caaaeaf..11e542f9633bee643daf5e88a2bd27f44bc06ec7 100644
--- a/Source/Geometry/Analytic/imstkCapsule.cpp
+++ b/Source/Geometry/Analytic/imstkCapsule.cpp
@@ -65,9 +65,9 @@ Capsule::getLength(DataType type /* = DataType::PostTransform */)
if (type == DataType::PostTransform)
{
this->updatePostTransformData();
- return m_radiusPostTransform;
+ return m_lengthPostTransform;
}
- return m_radius;
+ return m_length;
}
void
diff --git a/Source/Scene/imstkObjectInteractionFactory.cpp b/Source/Scene/imstkObjectInteractionFactory.cpp
index 34b4fc302311904e70c426cd03f89fc47fb91d72..80b9cd1ece2734e9f346739e61373aea98e776ea 100644
--- a/Source/Scene/imstkObjectInteractionFactory.cpp
+++ b/Source/Scene/imstkObjectInteractionFactory.cpp
@@ -26,6 +26,8 @@ limitations under the License.
#include "imstkFeDeformableObject.h"
#include "imstkPbdObject.h"
#include "imstkPbdObjectCollisionPair.h"
+#include "imstkPbdObjectPickingPair.h"
+#include "imstkPBDPickingCH.h"
#include "imstkPenaltyCH.h"
#include "imstkPickingCH.h"
#include "imstkImplicitGeometryToPointSetCD.h"
@@ -51,6 +53,10 @@ makeObjectInteractionPair(std::shared_ptr<CollidingObject> obj1, std::shared_ptr
{
results = std::make_shared<PbdObjectCollisionPair>(std::dynamic_pointer_cast<PbdObject>(obj1), std::dynamic_pointer_cast<PbdObject>(obj2), cdType);
}
+ else if (intType == InteractionType::PbdObjToCollidingObjPicking && isType<PbdObject>(obj1))
+ {
+ results = std::make_shared<PbdObjectPickingPair>(std::dynamic_pointer_cast<PbdObject>(obj1), std::dynamic_pointer_cast<CollidingObject>(obj2), cdType);
+ }
//else if (intType == InteractionType::PbdObjToCollidingObjCollision && isType<PbdObject>(obj1))
//{
// results = std::make_shared<PbdCollidingObjCollisionPair>(
diff --git a/Source/Scene/imstkObjectInteractionFactory.h b/Source/Scene/imstkObjectInteractionFactory.h
index 101b0f7301d8d33418809125ed3289d3b6acee76..e24cb78f6ddbf11db61ad63c3450e39b72306a4c 100644
--- a/Source/Scene/imstkObjectInteractionFactory.h
+++ b/Source/Scene/imstkObjectInteractionFactory.h
@@ -32,6 +32,7 @@ class ObjectInteractionPair;
enum class InteractionType
{
PbdObjToPbdObjCollision,
+ PbdObjToCollidingObjPicking,
PbdObjToCollidingObjCollision,
SphObjToCollidingObjCollision,
diff --git a/Source/Scene/imstkPbdObjectPickingPair.cpp b/Source/Scene/imstkPbdObjectPickingPair.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c8a96c990dbbc2e67593425c0665a3f3ba93681b
--- /dev/null
+++ b/Source/Scene/imstkPbdObjectPickingPair.cpp
@@ -0,0 +1,52 @@
+/*=========================================================================
+
+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 "imstkPbdObjectPickingPair.h"
+#include "imstkCDObjectFactory.h"
+#include "imstkCollisionData.h"
+#include "imstkPBDPickingCH.h"
+#include "imstkPbdModel.h"
+#include "imstkPbdObject.h"
+#include "imstkPbdSolver.h"
+#include "imstkTaskGraph.h"
+
+namespace imstk
+{
+// Pbd Collision will be tested before any step of pbd, then resolved after the solve steps of the two objects
+PbdObjectPickingPair::PbdObjectPickingPair(std::shared_ptr<PbdObject> obj1, std::shared_ptr<CollidingObject> obj2,
+ CollisionDetection::Type cdType) : CollisionPair(obj1, obj2)
+{
+ // Setup the CD
+ m_colData = std::make_shared<CollisionData>();
+ setCollisionDetection(makeCollisionDetectionObject(cdType, obj1->getCollidingGeometry(), obj2->getCollidingGeometry(), m_colData));
+
+ // Setup the handler
+ std::shared_ptr<PBDPickingCH> ch = std::make_shared<PBDPickingCH>(CollisionHandling::Side::A, m_colData, obj1, obj2);
+ setCollisionHandlingA(ch);
+}
+
+void
+PbdObjectPickingPair::apply()
+{
+ // Add the collision interaction
+ CollisionPair::apply();
+}
+}
\ No newline at end of file
diff --git a/Source/Scene/imstkPbdObjectPickingPair.h b/Source/Scene/imstkPbdObjectPickingPair.h
new file mode 100644
index 0000000000000000000000000000000000000000..ecfe54b9c40f6ef05c4ec052880e68c448fd0ed3
--- /dev/null
+++ b/Source/Scene/imstkPbdObjectPickingPair.h
@@ -0,0 +1,52 @@
+/*=========================================================================
+
+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 "imstkCollisionPair.h"
+#include "imstkCollisionDetection.h"
+
+namespace imstk
+{
+class PbdObject;
+class PbdSolver;
+
+///
+/// \class PbdObjectCollisionPair
+///
+/// \brief This class defines a collision interaction between two PbdObjects
+///
+class PbdObjectPickingPair : public CollisionPair
+{
+public:
+ PbdObjectPickingPair(std::shared_ptr<PbdObject> obj1, std::shared_ptr<CollidingObject> obj2,
+ CollisionDetection::Type cdType = CollisionDetection::Type::PointSetToCapsule);
+
+ virtual ~PbdObjectPickingPair() override = default;
+
+ void apply() override;
+
+private:
+ // Pbd defines two interactions (one at CD and one at solver)
+ Inputs m_solveNodeInputs;
+ Outputs m_solveNodeOutputs;
+};
+}
\ No newline at end of file