diff --git a/Base/Collision/CollisionHandling/imstkCollisionHandling.cpp b/Base/Collision/CollisionHandling/imstkCollisionHandling.cpp
index cf8348b623b2ba711973a777d59ac22a1aee06dd..8d4f1aec9190c8eb4dd4f1d7b69c2181140aa494 100644
--- a/Base/Collision/CollisionHandling/imstkCollisionHandling.cpp
+++ b/Base/Collision/CollisionHandling/imstkCollisionHandling.cpp
@@ -23,6 +23,8 @@
#include "imstkPenaltyCH.h"
#include "imstkVirtualCouplingCH.h"
+#include "imstkPickingCH.h"
+#include "imstkDeformableObject.h"
#include <g3log/g3log.hpp>
@@ -48,10 +50,34 @@ CollisionHandling::make_collision_handling(const Type& type,
{
case Type::None:
return nullptr;
+
case Type::Penalty:
+ if (objA->getType() == SceneObject::Type::Visual)
+ {
+ LOG(WARNING) << "CollisionHandling::make_collision_handling error: "
+ << "penalty collision handling only implemented for colliding objects.";
+ return nullptr;
+ }
+
return std::make_shared<PenaltyCH>(side, colData, objA);
+
case Type::VirtualCoupling:
+
return std::make_shared<VirtualCouplingCH>(side, colData, objA );
+
+ case Type::NodalPicking:
+
+ if (objA->getType() == SceneObject::Type::Visual)
+ {
+ LOG(WARNING) << "CollisionHandling::make_collision_handling error: "
+ << "penalty collision handling only implemented for colliding objects.";
+ return nullptr;
+ }
+ if (auto defObj = std::dynamic_pointer_cast<DeformableObject>(objA))
+ {
+ return std::make_shared<PickingCH>(side, colData, defObj);
+ }
+
default:
LOG(WARNING) << "CollisionHandling::make_collision_handling error: type not implemented.";
return nullptr;
diff --git a/Base/Collision/CollisionHandling/imstkCollisionHandling.h b/Base/Collision/CollisionHandling/imstkCollisionHandling.h
index 2725248e7b1998e0fde62e5517a2088a0fe8ff41..556782cb4f44d13b8d7d2d32e18dd603808c0aa3 100644
--- a/Base/Collision/CollisionHandling/imstkCollisionHandling.h
+++ b/Base/Collision/CollisionHandling/imstkCollisionHandling.h
@@ -49,7 +49,8 @@ public:
{
None,
Penalty,
- VirtualCoupling
+ VirtualCoupling,
+ NodalPicking
};
///
diff --git a/Base/Collision/CollisionHandling/imstkPickingCH.cpp b/Base/Collision/CollisionHandling/imstkPickingCH.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fa0d8f8b84ec972a07d19473dc1689d058e37808
--- /dev/null
+++ b/Base/Collision/CollisionHandling/imstkPickingCH.cpp
@@ -0,0 +1,81 @@
+/*=========================================================================
+
+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 "imstkPickingCH.h"
+#include "imstkDeformableObject.h"
+
+namespace imstk
+{
+
+void
+PickingCH::computeContactForces()
+{
+ if (m_object)
+ {
+ this->addPickConstraints(m_object);
+ }
+ else
+ {
+ LOG(WARNING) << "PickingCH::handleCollision error: "
+ << "no picking collision handling available the object";
+ }
+}
+
+void
+PickingCH::addPickConstraints(std::shared_ptr<DeformableObject> deformableObj)
+{
+ m_DynamicLinearProjConstraints->clear();
+
+ if (m_colData.NodePickData.empty())
+ {
+ return;
+ }
+
+ if (deformableObj == nullptr)
+ {
+ LOG(WARNING) << "PenaltyRigidCH::addPickConstraints error: "
+ << " not a deformable object.";
+ return;
+ }
+
+ const auto& Uprev = deformableObj->getDisplacements();
+ const auto& Vprev = deformableObj->getVelocities();
+
+ auto PhysTetMesh = std::dynamic_pointer_cast<Mesh>(deformableObj->getPhysicsGeometry());
+ auto dT = std::dynamic_pointer_cast<FEMDeformableBodyModel>(m_object->getDynamicalModel())->getTimeIntegrator()->getTimestepSize();
+
+ // If collision data, append LPC constraints
+ for (const auto& CD : m_colData.NodePickData)
+ {
+ auto nodeDof = 3 * CD.nodeId;
+ auto vprev = Vec3d(Vprev(nodeDof), Vprev(nodeDof + 1), Vprev(nodeDof + 2));
+ auto uprev = Vec3d(Uprev(nodeDof), Uprev(nodeDof + 1), Uprev(nodeDof + 2));
+ auto x = (CD.ptPos + PhysTetMesh->getVertexPosition(CD.nodeId) -
+ PhysTetMesh->getInitialVertexPosition(CD.nodeId) - uprev) / dT - vprev;
+
+ auto pickProjector = LinearProjectionConstraint(CD.nodeId, true);
+ pickProjector.setProjectorToDirichlet(CD.nodeId, x);
+
+ m_DynamicLinearProjConstraints->push_back(pickProjector);
+ }
+}
+
+}
\ No newline at end of file
diff --git a/Base/Collision/CollisionHandling/imstkPickingCH.h b/Base/Collision/CollisionHandling/imstkPickingCH.h
new file mode 100644
index 0000000000000000000000000000000000000000..88113eba1e36bb98674a442d875677e8032dae26
--- /dev/null
+++ b/Base/Collision/CollisionHandling/imstkPickingCH.h
@@ -0,0 +1,96 @@
+/*=========================================================================
+
+ Library: iMSTK
+
+ Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+ & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0.txt
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+
+ =========================================================================*/
+
+#ifndef imstkPickingCH_h
+#define imstkPenaltyCH_h
+
+// std library
+#include <memory>
+
+// imstk
+#include "imstkCollisionHandling.h"
+#include "imstkNonlinearSystem.h"
+
+namespace imstk
+{
+
+class CollidingObject;
+class DeformableObject;
+class CollisionData;
+
+///
+/// \class PickingCH
+///
+/// \brief Implements nodal picking
+///
+class PickingCH : public CollisionHandling
+{
+public:
+
+ ///
+ /// \brief Constructor
+ ///
+ PickingCH(const Side& side,
+ const CollisionData& colData,
+ std::shared_ptr<DeformableObject> obj):
+ CollisionHandling(Type::NodalPicking, side, colData),
+ m_object(obj){}
+
+ PickingCH() = delete;
+
+ ///
+ /// \brief Destructor
+ ///
+ ~PickingCH() = default;
+
+ ///
+ /// \brief Compute forces based on collision data
+ ///
+ void computeContactForces() override;
+
+ ///
+ /// \brief Add LPC constraints for the node that is picked
+ ///
+ void addPickConstraints(std::shared_ptr<DeformableObject> deformableObj);
+
+ /// \brief Get the vector denoting the filter
+ ///
+ void setDynamicLinearProjectors(std::vector<LinearProjectionConstraint>* f)
+ {
+ m_DynamicLinearProjConstraints = f;
+ }
+
+ /// \brief Get the vector denoting the filter
+ ///
+ std::vector<LinearProjectionConstraint>& getDynamicLinearProjectors()
+ {
+ return *m_DynamicLinearProjConstraints;
+ }
+
+private:
+
+ std::shared_ptr<DeformableObject> m_object; ///> Deformable object
+ std::vector<LinearProjectionConstraint> *m_DynamicLinearProjConstraints;
+};
+
+}
+
+#endif // ifndef imstkPenaltyCH_h
\ No newline at end of file