Merge branch 'PBD-picking-handling' into 'master'

Pbd Picking Handling & Laparoscopic Tool Updates

See merge request iMSTK/iMSTK!509

Data/laptool/lower.obj.mtl.sha512

+c194f28a2e4a48a6a01439f0d5a7b3f4073887829529820e9c82232b83a7ab66962419a37f0cb7ae4b5673e094e8f39a5c1a99645f32c7c84bf1286a4df3dbf9
\ No newline at end of file

Data/laptool/lower.obj.sha512

-eab8e05fbbfa17a05b680429142d27e7021f16b90927aba8b04341718a4e56344f2e113cc3861fb77ff74774c3f99e3db03a636b3c6bc6447fdcc352e0986f0a
\ No newline at end of file
+c5d915dc6eb1aa7907aba0bf0dd558a551017ae46aa3651d523ba614248a32c100427030da890549f9397e8941cf5ee8cd39b0fa9300d71c5bf3fb7ec4f22440
\ No newline at end of file

Data/laptool/pivot.obj.mtl.sha512

+4c1ce8e1fb894be9a295b22824f999c8cd7099d59c1e8239dd025cb8dda8fc53da4a60351dd1d25aa32101365f4557944d0664e96d7ea81b8e9f9794568101ee
\ No newline at end of file

Data/laptool/pivot.obj.sha512

-3f0838c3cd99192ed22004aaab9090f023513be0f0c3af086a3a5ab487b40ba9da7e319269a4cf2c36b905b840adff47cbf19c5d403c63414d68779a60c0e7f8
\ No newline at end of file
+8655ed1b030b9101aaf7543727de275312e2e29f7aa2310ae1565169fe0ef49801eb87bfe617132324e03fe65e99f8b7c54518a196575c87d96d3babfc649ffe
\ No newline at end of file

Data/laptool/upper.obj.mtl.sha512

+9585d6e768cf02902b53afc772e212a7d111d5c37724db55ca316cd44ab1bae5670bd5f12ed76516dc88ab02638e2fba507d8a2d169e39af8ca96841c7885925
\ No newline at end of file

Data/laptool/upper.obj.sha512

-6b55f6d9ec3d16a6385faffb181c5579bcb7acc6405953599abd603d45b72153542175436ba2a3f02ee8fbb2d1b122538d91a8a00d3284b4e94e0fb429c95385
\ No newline at end of file
+9947ea15f111d68f11bdd51d06019cb12932a10d0ee4c2e8d1410e7733d6938d483d6373e258c6fbcee60df3daf0cdab68ef5415b905bcc94df7d06a5bd88a86
\ No newline at end of file

Examples/PBDPicking/CMakeLists.txt

+###########################################################################
+#
+# 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()

Examples/PBDPicking/PBDPickingExample.cpp

+/*=========================================================================
+
+   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;
+}

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);

Source/CollisionHandling/imstkCollisionHandling.h

@@ -51,6 +51,7 @@ public:
         BoneDrilling,
         SPH,
         PBD,
+        PBDPicking,
         RBD,
         LevelSet
     };

Source/CollisionHandling/imstkPBDPickingCH.cpp

+/*=========================================================================
+
+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;
+    }
+}
+}

Source/CollisionHandling/imstkPBDPickingCH.h

+/*=========================================================================
+
+   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
+};
+}

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);
+}
 }

Source/Controllers/imstkLaparoscopicToolController.cpp

-/*=========================================================================
-
-   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

Source/Controllers/imstkLaparoscopicToolController.h

-/*=========================================================================
-
-   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

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()
 {

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

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

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>(

Source/Scene/imstkObjectInteractionFactory.h

@@ -32,6 +32,7 @@ class ObjectInteractionPair;
 enum class InteractionType
 {
     PbdObjToPbdObjCollision,
+    PbdObjToCollidingObjPicking,
 
     PbdObjToCollidingObjCollision,
     SphObjToCollidingObjCollision,