diff --git a/Base/Collision/CollisionHandling/imstkBoneDrillingCH.cpp b/Base/Collision/CollisionHandling/imstkBoneDrillingCH.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cb6bd1b26fb0a3cb9f06278d39687f510cf63209
--- /dev/null
+++ b/Base/Collision/CollisionHandling/imstkBoneDrillingCH.cpp
@@ -0,0 +1,159 @@
+/*=========================================================================
+
+ 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 "imstkBoneDrillingCH.h"
+
+#include "imstkCollidingObject.h"
+#include "imstkTetrahedralMesh.h"
+#include "imstkCollisionData.h"
+#include "imstkDeviceTracker.h"
+#include "imstkMath.h"
+
+#include <g3log/g3log.hpp>
+
+namespace imstk
+{
+
+BoneDrillingCH::BoneDrillingCH(const Side& side,
+ const CollisionData& colData,
+ std::shared_ptr<CollidingObject> bone,
+ std::shared_ptr<CollidingObject> drill) :
+ CollisionHandling(Type::BoneDrilling, side, colData),
+ m_drill(drill),
+ m_bone(bone)
+{
+ auto boneMesh = std::dynamic_pointer_cast<TetrahedralMesh>(m_bone->getCollidingGeometry());
+
+ if (!boneMesh)
+ {
+ LOG(WARNING) << "BoneDrillingCH::BoneDrillingCH Error:The bone colliding geometry is not a mesh!";
+ }
+
+ // Initialize bone density values
+ for (int i = 0; i < boneMesh->getNumVertices(); ++i)
+ {
+ m_nodalDensity.push_back(m_initialBoneDensity);
+ }
+
+ for (int i = 0; i < boneMesh->getNumVertices(); ++i)
+ {
+ m_nodeRemovalStatus.push_back(false);
+ }
+
+ m_nodalCardinalSet.resize(boneMesh->getNumVertices());
+ for (int i = 0; i < boneMesh->getNumVertices(); ++i)
+ {
+ std::vector<int> row;
+ m_nodalCardinalSet.push_back(row);
+ }
+
+ // Pre-compute the nodal cardinality set
+ for (int i = 0; i < boneMesh->getNumTetrahedra(); ++i)
+ {
+ for (auto& vert : boneMesh->getTetrahedronVertices(i))
+ {
+ m_nodalCardinalSet[vert].push_back(i);
+ }
+ }
+
+
+}
+
+void
+BoneDrillingCH::erodeBone()
+{
+ auto boneTetMesh = std::dynamic_pointer_cast<TetrahedralMesh>(m_bone->getCollidingGeometry());
+
+ for (auto& cd : m_colData.MAColData)
+ {
+ if (m_nodeRemovalStatus[cd.nodeId])
+ {
+ continue;
+ }
+
+ m_nodalDensity[cd.nodeId] -= 0.001*(m_angularSpeed / m_BoneHardness)*m_stiffness*cd.penetrationVector.norm()*0.001;
+
+ if (m_nodalDensity[cd.nodeId] <= 0.)
+ {
+ m_erodedNodes.push_back(cd.nodeId);
+ m_nodeRemovalStatus[cd.nodeId] = true;
+
+ // tag the tetra that will be removed
+ for (auto& tetId : m_nodalCardinalSet[cd.nodeId])
+ {
+ boneTetMesh->setTetrahedraAsRemoved(tetId);
+ boneTetMesh->setTopologyChangedFlag(true);
+ }
+ }
+ }
+}
+
+void
+BoneDrillingCH::computeContactForces()
+{
+ // Check if any collisions
+ const auto devicePosition = m_drill->getCollidingGeometry()->getTranslation();
+ if (m_colData.MAColData.empty())
+ {
+ // Set the visual object position same as the colliding object position
+ m_drill->getVisualGeometry()->setTranslation(devicePosition);
+ return;
+ }
+
+ // Update visual object position
+
+ // Aggregate collision data
+ Vec3d t = Vec3d::Zero();
+ double maxDepth = MIN_D;
+ for (const auto& cd : m_colData.MAColData)
+ {
+ if (m_nodeRemovalStatus[cd.nodeId])
+ {
+ continue;
+ }
+
+ if (cd.penetrationVector.norm() > maxDepth)
+ {
+ maxDepth = cd.penetrationVector.norm();
+ t = cd.penetrationVector;
+ }
+ }
+ m_drill->getVisualGeometry()->setTranslation(devicePosition + t);
+
+ // Spring force
+ Vec3d force = m_stiffness * (m_drill->getVisualGeometry()->getTranslation() - devicePosition);
+
+ // Damping force
+ const double dt = 0.1; // Time step size to calculate the object velocity
+ force += m_initialStep ? Vec3d(0.0, 0.0, 0.0) : m_damping * (devicePosition - m_prevPos) / dt;
+
+ // Update object contact force
+ m_drill->appendForce(force);
+
+ // Decrease the density at the nodal points and remove if the density goes below 0
+ this->erodeBone();
+
+ // Housekeeping
+ m_initialStep = false;
+ m_prevPos = devicePosition;
+}
+
+}// iMSTK
\ No newline at end of file
diff --git a/Base/Collision/CollisionHandling/imstkBoneDrillingCH.h b/Base/Collision/CollisionHandling/imstkBoneDrillingCH.h
new file mode 100644
index 0000000000000000000000000000000000000000..5e3e33055c42cc85a2207b85ecf42ec6db37bab2
--- /dev/null
+++ b/Base/Collision/CollisionHandling/imstkBoneDrillingCH.h
@@ -0,0 +1,106 @@
+/*=========================================================================
+
+ 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 ismtkBoneDrillingCH_h
+#define imstkBoneDrillingCH_h
+
+// imstk
+#include "imstkCollisionHandling.h"
+
+
+namespace imstk
+{
+
+class CollidingObject;
+class CollisionData;
+class DeviceTracker;
+
+///
+/// \class BoneDrillingCH
+///
+/// \brief Implements bone drilling collision handling
+///
+class BoneDrillingCH : public CollisionHandling
+{
+public:
+
+ ///
+ /// \brief Constructor
+ ///
+ BoneDrillingCH(const Side& side,
+ const CollisionData& colData,
+ std::shared_ptr<CollidingObject> bone,
+ std::shared_ptr<CollidingObject> drill);
+
+ BoneDrillingCH() = delete;
+
+ ///
+ /// \brief Destructor
+ ///
+ ~BoneDrillingCH() = default;
+
+ ///
+ /// \brief Decrease the density at the nodal points and remove if the density goes below 0
+ ///
+ void erodeBone();
+
+ ///
+ /// \brief Compute forces based on collision data
+ ///
+ void computeContactForces() override;
+
+ ///
+ /// \brief Get stiffness
+ ///
+ inline const double getStiffness() const { return m_stiffness; }
+ inline void setStiffness(const double k) { m_stiffness = k; }
+
+ ///
+ /// \brief Get damping coefficient
+ ///
+ inline const double getDamping() const { return m_damping; }
+ inline void setDamping(const double d) { m_damping = d; }
+
+private:
+ std::shared_ptr<CollidingObject> m_bone; ///> bone object
+ std::shared_ptr<CollidingObject> m_drill; ///> drill object
+
+ double m_stiffness = 10e-01; ///> Stiffness coefficient associated with virtual coupling object
+ double m_damping = 0.005; ///> Damping coefficient associated with virtual coupling object
+
+ double m_angularSpeed = 10*PI; ///> Angular speed of the drill (rad per sec)
+ double m_BoneHardness = 10; ///> Angular speed of the drill (rad per sec)
+
+ std::vector<double> m_nodalDensity; ///> Density of the bone
+ double m_initialBoneDensity = 1.0; ///> Density of the bone before the start of the drilling process
+
+ std::vector<int> m_erodedNodes;
+ std::vector<bool> m_nodeRemovalStatus; ///> Keeps track of the removal status of the node
+ std::vector<std::vector<int>> m_nodalCardinalSet; ///> Keeps track of the removal status of the node
+
+ bool m_initialStep = true; ///> Number of times steps
+ Vec3d m_prevPos; ///> Previous position of the colliding object
+
+};
+
+}
+
+#endif // ifndef imstkBoneDrillingCH_h
\ No newline at end of file
diff --git a/Base/Collision/CollisionHandling/imstkCollisionHandling.cpp b/Base/Collision/CollisionHandling/imstkCollisionHandling.cpp
index 8d4f1aec9190c8eb4dd4f1d7b69c2181140aa494..014a5530e447a93a1476322f2dba4ce7ee921166 100644
--- a/Base/Collision/CollisionHandling/imstkCollisionHandling.cpp
+++ b/Base/Collision/CollisionHandling/imstkCollisionHandling.cpp
@@ -25,6 +25,7 @@
#include "imstkVirtualCouplingCH.h"
#include "imstkPickingCH.h"
#include "imstkDeformableObject.h"
+#include "imstkBoneDrillingCH.h"
#include <g3log/g3log.hpp>
@@ -78,6 +79,9 @@ CollisionHandling::make_collision_handling(const Type& type,
return std::make_shared<PickingCH>(side, colData, defObj);
}
+ case Type::BoneDrilling:
+ return std::make_shared<BoneDrillingCH>(side, colData, objA, objB);
+
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 556782cb4f44d13b8d7d2d32e18dd603808c0aa3..7d86c74f0c39625e3cfa5a4b2f4049197dd655b0 100644
--- a/Base/Collision/CollisionHandling/imstkCollisionHandling.h
+++ b/Base/Collision/CollisionHandling/imstkCollisionHandling.h
@@ -50,7 +50,8 @@ public:
None,
Penalty,
VirtualCoupling,
- NodalPicking
+ NodalPicking,
+ BoneDrilling
};
///