diff --git a/Base/Collision/CMakeLists.txt b/Base/Collision/CMakeLists.txt
index 5322eb1b193f659ba3f65c0168bb259e7a952272..b7aafdff75390daf58e7008c9dab557a3cd3db86 100644
--- a/Base/Collision/CMakeLists.txt
+++ b/Base/Collision/CMakeLists.txt
@@ -7,6 +7,7 @@ imstk_add_library( Collision
SCCD
Geometry
SceneElements
+ DynamicalModels
)
#-----------------------------------------------------------------------------
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
};
///
diff --git a/Base/Collision/CollisionHandling/imstkPickingCH.h b/Base/Collision/CollisionHandling/imstkPickingCH.h
index 88113eba1e36bb98674a442d875677e8032dae26..2465706ef8dfac5b1cbd83f24210df8659674f64 100644
--- a/Base/Collision/CollisionHandling/imstkPickingCH.h
+++ b/Base/Collision/CollisionHandling/imstkPickingCH.h
@@ -71,6 +71,7 @@ public:
///
void addPickConstraints(std::shared_ptr<DeformableObject> deformableObj);
+ ///
/// \brief Get the vector denoting the filter
///
void setDynamicLinearProjectors(std::vector<LinearProjectionConstraint>* f)
@@ -78,6 +79,7 @@ public:
m_DynamicLinearProjConstraints = f;
}
+ ///
/// \brief Get the vector denoting the filter
///
std::vector<LinearProjectionConstraint>& getDynamicLinearProjectors()
diff --git a/Base/Geometry/Mesh/imstkMesh.h b/Base/Geometry/Mesh/imstkMesh.h
index 9e53fb6ec5c0eaad74321c4fb0d3973849cde1cc..31d839f5f287e3af7ed5cee6775331f968f67f4e 100644
--- a/Base/Geometry/Mesh/imstkMesh.h
+++ b/Base/Geometry/Mesh/imstkMesh.h
@@ -139,6 +139,12 @@ public:
///
const size_t getNumVertices() const;
+ ///
+ /// \brief Set the topologyChanged flag
+ ///
+ void setTopologyChangedFlag(const bool flag) { m_topologyChanged = flag; }
+ bool getTopologyChangedFlag() const { return m_topologyChanged; };
+
protected:
///
@@ -169,6 +175,8 @@ protected:
StdVectorOfVec3d m_vertexPositionsPostTransform; ///> Positions of vertices after transform
std::map<std::string, StdVectorOfVectorf> m_pointDataMap; ///> vector of data arrays per vertice
+
+ bool m_topologyChanged = false;
};
} // imstk
diff --git a/Base/Geometry/Mesh/imstkTetrahedralMesh.cpp b/Base/Geometry/Mesh/imstkTetrahedralMesh.cpp
index 6e1d8af197b64e1f05bcc58a8e929f02c890d5e7..ae434332ca722a040d33b32a2cf154952d4c19b3 100644
--- a/Base/Geometry/Mesh/imstkTetrahedralMesh.cpp
+++ b/Base/Geometry/Mesh/imstkTetrahedralMesh.cpp
@@ -36,6 +36,13 @@ TetrahedralMesh::initialize(const StdVectorOfVec3d& vertices,
{
this->computeAttachedSurfaceMesh();
}
+
+ //m_removedMeshElems.resize(tetrahedra.size());
+ for (int i = 0; i < tetrahedra.size(); ++i)
+ {
+ m_removedMeshElems.push_back(false);
+ }
+ //m_removedMeshElems.assign(false, m_removedMeshElems.size());
}
void
diff --git a/Base/Geometry/Mesh/imstkTetrahedralMesh.h b/Base/Geometry/Mesh/imstkTetrahedralMesh.h
index 7c007afdc003208b8a951e29d078385699cceff1..259117d04ed434c48aba9b27efff7a6219e4497c 100644
--- a/Base/Geometry/Mesh/imstkTetrahedralMesh.h
+++ b/Base/Geometry/Mesh/imstkTetrahedralMesh.h
@@ -121,8 +121,16 @@ public:
///
size_t getNumTetrahedra() const;
+ ///
+ /// \brief Get/set method for removed elements from the mesh
+ ///
+ void setTetrahedraAsRemoved(const unsigned int tetId){ m_removedMeshElems[tetId] = true; }
+ const std::vector<bool>& getRemovedTetrahedra() const { return m_removedMeshElems; }
+
protected:
- std::vector<TetraArray> m_tetrahedraVertices; ///< vertices of the tetrahedra
+ std::vector<TetraArray> m_tetrahedraVertices;///< vertices of the tetrahedra
+
+ std::vector<bool> m_removedMeshElems;
};
}
diff --git a/Base/Rendering/RenderDelegate/imstkVTKTetrahedralMeshRenderDelegate.cpp b/Base/Rendering/RenderDelegate/imstkVTKTetrahedralMeshRenderDelegate.cpp
index f639cd57f8ee89bbe41dda0d7b491c1df7c41050..93cdd276e3bb431b3b17be50693df15ce9d76cd8 100644
--- a/Base/Rendering/RenderDelegate/imstkVTKTetrahedralMeshRenderDelegate.cpp
+++ b/Base/Rendering/RenderDelegate/imstkVTKTetrahedralMeshRenderDelegate.cpp
@@ -28,6 +28,11 @@
#include <vtkPoints.h>
#include <vtkDoubleArray.h>
#include <vtkCellArray.h>
+#include <vtkProperty.h>
+#include <vtkPolyData.h>
+#include <vtkPointData.h>
+#include <vtkFloatArray.h>
+#include <vtkNew.h>
namespace imstk
{
@@ -60,14 +65,15 @@ VTKTetrahedralMeshRenderDelegate::VTKTetrahedralMeshRenderDelegate(std::shared_p
}
// Create Unstructured Grid
- auto unstructuredGrid = vtkSmartPointer<vtkUnstructuredGrid>::New();
- unstructuredGrid->SetPoints(points);
- unstructuredGrid->SetCells(VTK_TETRA, cells);
- m_geometry->m_dataModified = false;
+ m_mesh = vtkSmartPointer<vtkUnstructuredGrid>::New();
+ m_mesh->SetPoints(points);
+ m_mesh->SetCells(VTK_TETRA, cells);
// Mapper & Actor
auto mapper = vtkSmartPointer<vtkDataSetMapper>::New();
- mapper->SetInputData(unstructuredGrid);
+ mapper->SetInputData(m_mesh);
+
+ // Actor
m_actor->SetMapper(mapper);
// Update Transform, Render Properties
@@ -82,9 +88,35 @@ VTKTetrahedralMeshRenderDelegate::updateDataSource()
return;
}
- m_mappedVertexArray->Modified();
+ if (m_geometry->getTopologyChangedFlag())
+ {
+ m_mappedVertexArray->Modified(); // TODO: only modify if vertices change
+
+ // Copy cells
+ auto& maskedTets = std::dynamic_pointer_cast<TetrahedralMesh>(m_geometry)->getRemovedTetrahedra();
+
+ auto cells = vtkSmartPointer<vtkCellArray>::New();
+ vtkIdType cell[4];
+ size_t tetId = 0;
- m_geometry->m_dataModified = false;
+ // Assign new cells
+ for (const auto &t : m_geometry->getTetrahedraVertices())
+ {
+ if (!maskedTets[tetId])
+ {
+ for (size_t i = 0; i < 4; ++i)
+ {
+ cell[i] = t[i];
+ }
+ cells->InsertNextCell(4, cell);
+ }
+
+ tetId++;
+ }
+ m_mesh->SetCells(VTK_TETRA, cells);
+ m_geometry->setTopologyChangedFlag(false);
+ }
+ m_geometry->m_dataModified = true;
}
diff --git a/Base/Rendering/RenderDelegate/imstkVTKTetrahedralMeshRenderDelegate.h b/Base/Rendering/RenderDelegate/imstkVTKTetrahedralMeshRenderDelegate.h
index 5e299b8f6797b96dc58d4736f1c60dc2f6fea874..a19469cd4c81835ff62b7ad3d073cdccf17fccdb 100644
--- a/Base/Rendering/RenderDelegate/imstkVTKTetrahedralMeshRenderDelegate.h
+++ b/Base/Rendering/RenderDelegate/imstkVTKTetrahedralMeshRenderDelegate.h
@@ -27,6 +27,7 @@
#include "imstkVTKRenderDelegate.h"
class vtkDoubleArray;
+class vtkUnstructuredGrid;
namespace imstk
{
@@ -63,8 +64,9 @@ public:
protected:
- std::shared_ptr<TetrahedralMesh> m_geometry; ///> Geometry to render
- vtkSmartPointer<vtkDoubleArray> m_mappedVertexArray; ///> Mapped array of vertices
+ std::shared_ptr<TetrahedralMesh> m_geometry; ///> Geometry to render
+ vtkSmartPointer<vtkDoubleArray> m_mappedVertexArray; ///> Mapped array of vertices
+ vtkSmartPointer<vtkUnstructuredGrid> m_mesh; ///> Mapped tetrahedral mesh
};
} // imstk
diff --git a/Examples/Sandbox/main.cpp b/Examples/Sandbox/main.cpp
index 15b3441535d02e203de6acb89ce6db55fbe9b07f..b96ed3cc820d90d79a4baf9b2f6f3cd8ae4fc48e 100644
--- a/Examples/Sandbox/main.cpp
+++ b/Examples/Sandbox/main.cpp
@@ -32,6 +32,7 @@
// Objects
#include "imstkForceModelConfig.h"
#include "imstkFEMDeformableBodyModel.h"
+#include "imstkDynamicObject.h"
#include "imstkDeformableObject.h"
#include "imstkSceneObject.h"
#include "imstkLight.h"
@@ -77,6 +78,7 @@
#include "imstkVirtualCouplingCH.h"
#include "imstkMeshToSpherePickingCD.h"
#include "imstkPickingCH.h"
+#include "imstkBoneDrillingCH.h"
// logger
#include "g3log/g3log.hpp"
@@ -133,6 +135,7 @@ void testCapsule();
void testVirtualCoupling();
void testGeometryTransforms();
void testPicking();
+void testBoneDrilling();
int main()
{
@@ -204,6 +207,7 @@ int main()
//testScenesManagement();
//testVectorPlotters();
//testVirtualCoupling();
+ testBoneDrilling();
return 0;
@@ -2577,6 +2581,7 @@ void testGeometryTransforms()
sdk->startSimulation(false);
}
+
void testPicking()
{
// SDK and Scene
@@ -2587,7 +2592,7 @@ void testPicking()
// Create plane visual scene object
//----------------------------------------------------------
auto planeObj = apiutils::createVisualAnalyticalSceneObject(
- imstk::Geometry::Type::Plane, scene, "VisualPlane", 100, Vec3d(0., -20., 0.));
+ imstk::Geometry::Type::Plane, scene, "VisualPlane", 100, Vec3d(0., -20., 0.));
//----------------------------------------------------------
// Create Nidus FE deformable scene object
@@ -2705,3 +2710,70 @@ void testPicking()
sdk->setCurrentScene(scene);
sdk->startSimulation(true);
}
+
+
+void testBoneDrilling()
+{
+ // SDK and Scene
+ auto sdk = std::make_shared<imstk::SimulationManager>();
+ auto scene = sdk->createNewScene("BoneDrilling");
+
+ // Add virtual coupling object in the scene.
+#ifdef iMSTK_USE_OPENHAPTICS
+
+ // Device clients
+ auto client = std::make_shared<imstk::HDAPIDeviceClient>("Default Device");
+
+ // Device Server
+ auto server = std::make_shared<imstk::HDAPIDeviceServer>();
+ server->addDeviceClient(client);
+ sdk->addModule(server);
+
+ // Device tracker
+ auto deviceTracker = std::make_shared<imstk::DeviceTracker>(client);
+
+ // Create bone scene object
+ // Load the mesh
+ auto tetMesh = imstk::MeshIO::read(iMSTK_DATA_ROOT"/asianDragon/asianDragon.veg");
+ if (!tetMesh)
+ {
+ LOG(WARNING) << "Could not read mesh from file.";
+ return;
+ }
+ auto bone = std::make_shared<CollidingObject>("Bone");
+ bone->setCollidingGeometry(tetMesh);
+ bone->setVisualGeometry(tetMesh);
+ scene->addSceneObject(bone);
+
+ // Create a virtual coupling object: Drill
+ auto drillVisualGeom = std::make_shared<imstk::Sphere>();
+ drillVisualGeom->setRadius(3.);
+ auto drillCollidingGeom = std::make_shared<imstk::Sphere>();
+ drillCollidingGeom->setRadius(3.);
+ auto drill = std::make_shared<CollidingObject>("Drill");
+ drill->setCollidingGeometry(drillCollidingGeom);
+ drill->setVisualGeometry(drillVisualGeom);
+ scene->addSceneObject(drill);
+
+ // Create and add virtual coupling object controller in the scene
+ auto objController = std::make_shared<imstk::SceneObjectController>(drill, deviceTracker);
+ scene->addObjectController(objController);
+
+ // Create a collision graph
+ auto graph = scene->getCollisionGraph();
+ auto pair = graph->addInteractionPair(bone,
+ drill,
+ CollisionDetection::Type::MeshToSphere,
+ CollisionHandling::Type::BoneDrilling,
+ CollisionHandling::Type::None);
+
+#endif
+
+ //Run
+ auto cam = scene->getCamera();
+ cam->setPosition(imstk::Vec3d(0, 0, 15));
+
+ sdk->setCurrentScene(scene);
+ sdk->startSimulation(false);
+}
+