iMSTK
Interactive Medical Simulation Toolkit
All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Pages
imstkProximitySurfaceSelector.cpp
1 /*
2 ** This file is part of the Interactive Medical Simulation Toolkit (iMSTK)
3 ** iMSTK is distributed under the Apache License, Version 2.0.
4 ** See accompanying NOTICE for details.
5 */
6 
7 #include "imstkProximitySurfaceSelector.h"
8 #include "imstkGeometryUtilities.h"
9 #include "imstkLogger.h"
10 #include "imstkSurfaceMesh.h"
11 #include "imstkVecDataArray.h"
12 
13 namespace imstk
14 {
15 ProximitySurfaceSelector::ProximitySurfaceSelector()
16 {
18  setRequiredInputType<SurfaceMesh>(0);
19  setRequiredInputType<SurfaceMesh>(1);
20 
22  setOutput(std::shared_ptr<SurfaceMesh>(), 0);
23  setOutput(std::shared_ptr<SurfaceMesh>(), 1);
24 }
25 
26 void
27 ProximitySurfaceSelector::setInputMeshes(
28  std::shared_ptr<SurfaceMesh> inputMeshA,
29  std::shared_ptr<SurfaceMesh> inputMeshB)
30 {
31  setInput(inputMeshA, 0);
32  setInput(inputMeshB, 1);
33 }
34 
35 std::shared_ptr<SurfaceMesh>
36 ProximitySurfaceSelector::getOutputMeshA() const
37 {
38  return std::static_pointer_cast<SurfaceMesh>(getOutput(0));
39 }
40 
41 std::shared_ptr<SurfaceMesh>
42 ProximitySurfaceSelector::getOutputMeshB() const
43 {
44  return std::static_pointer_cast<SurfaceMesh>(getOutput(1));
45 }
46 
47 void
49 {
50  std::shared_ptr<SurfaceMesh> meshA = std::dynamic_pointer_cast<SurfaceMesh>(getInput(0));
51  std::shared_ptr<SurfaceMesh> meshB = std::dynamic_pointer_cast<SurfaceMesh>(getInput(1));
52 
53  // Handy storage for tracking which faces have been added
54  std::pair<std::vector<int>, std::vector<int>> closeSurfaces;
55 
56  // Handy storage for tracking and generating sub surface meshes
57  std::pair<std::shared_ptr<SurfaceMesh>, std::shared_ptr<SurfaceMesh>> subMeshes;
58 
59  subMeshes.first = std::make_shared<SurfaceMesh>();
60  subMeshes.second = std::make_shared<SurfaceMesh>();
61 
62  // Check minimum distance
63  double minDist = IMSTK_DOUBLE_MAX;
64  for (int vertId_a = 0; vertId_a < meshA->getNumVertices(); vertId_a++)
65  {
66  for (int vertId_b = 0; vertId_b < meshB->getNumVertices(); vertId_b++)
67  {
68  const auto& vertA = meshA->getVertexPosition(vertId_a);
69  const auto& vertB = meshB->getVertexPosition(vertId_b);
70  minDist = std::min(minDist, (vertA - vertB).norm());
71  }
72  }
73 
74  if (minDist > m_proximity)
75  {
76  LOG(WARNING) << "No SurfaceMeshes generated, the meshes are further apart than the requested proximity";
77  return;
78  }
79 
80  // Unpack cell and vertex data for meshA
81  std::shared_ptr<VecDataArray<int, 3>> meshACellsPtr = meshA->getCells();
82  VecDataArray<int, 3>& meshACells = *meshACellsPtr;
83  std::shared_ptr<VecDataArray<double, 3>> meshAVertsPtr = meshA->getVertexPositions();
84 
85  // Unpack cell and vertex data for meshB
86  std::shared_ptr<VecDataArray<int, 3>> meshBCellsPtr = meshB->getCells();
87  VecDataArray<int, 3>& meshBCells = *meshBCellsPtr;
88  std::shared_ptr<VecDataArray<double, 3>> meshBVertsPtr = meshB->getVertexPositions();
89 
90  // Check if the center of a triangle on Mesh A is within maxDistance of any
91  // triangle center on mesh B. If so, add to list. Also, vice versa.
92 
93  // Storage for vertex indices of sub triangle
94  auto subIndicesPtrA = std::make_shared<VecDataArray<int, 3>>();
95  VecDataArray<int, 3>& subIndicesA = *subIndicesPtrA;
96 
97  auto subIndicesPtrB = std::make_shared<VecDataArray<int, 3>>();
98  VecDataArray<int, 3>& subIndicesB = *subIndicesPtrB;
99 
100  for (int cellId_a = 0; cellId_a < meshACells.size(); cellId_a++)
101  {
102  // Calculate position of center of triangle
103  const Vec3i& triangleVertexIdsA = meshACells[cellId_a];
104  Vec3d cellACenter = (meshA->getVertexPosition(triangleVertexIdsA(0))
105  + meshA->getVertexPosition(triangleVertexIdsA(1))
106  + meshA->getVertexPosition(triangleVertexIdsA(2))) / 3.0;
107 
108  // Check if within maxDist of any cell on B
109  for (int cellId_b = 0; cellId_b < meshBCells.size(); cellId_b++)
110  {
111  const Vec3i& triangleVertexIdsB = meshBCells[cellId_b];
112  Vec3d cellBCenter = (meshB->getVertexPosition(triangleVertexIdsB(0))
113  + meshB->getVertexPosition(triangleVertexIdsB(1))
114  + meshB->getVertexPosition(triangleVertexIdsB(2))) / 3.0;
115 
116  if ((cellACenter - cellBCenter).squaredNorm() <= m_proximity * m_proximity)
117  {
118  // If this surface has already been added, skip
119  if (std::find(closeSurfaces.first.begin(), closeSurfaces.first.end(), cellId_a) == closeSurfaces.first.end())
120  {
121  closeSurfaces.first.push_back(cellId_a);
122  // If this triangle is within maxDist of Mesh B, add its vertex indiecs to subset
123  const Vec3i& triangleVertexIds = meshACells[cellId_a];
124  subIndicesA.push_back(triangleVertexIds);
125  }
126  if (std::find(closeSurfaces.second.begin(), closeSurfaces.second.end(), cellId_b) == closeSurfaces.second.end())
127  {
128  closeSurfaces.second.push_back(cellId_b);
129  // If this triangle is within maxDist of Mesh B, add its vertex indiecs to subset
130  const Vec3i& triangleVertexIds = meshBCells[cellId_b];
131  subIndicesB.push_back(triangleVertexIds);
132  }
133  }
134  }
135  }
136 
137  // Initialize submesh from mesh A
138  subMeshes.first->initialize(meshAVertsPtr, subIndicesPtrA);
139  setOutput(subMeshes.first, 0);
140 
141  // Initialize submesh from mesh B
142  subMeshes.second->initialize(meshBVertsPtr, subIndicesPtrB);
143  setOutput(subMeshes.second, 1);
144 }
145 } // namespace imstk
void requestUpdate() override
Users can implement this for the logic to be run.
std::shared_ptr< Geometry > getInput(size_t port=0) const
Returns input geometry given port, returns nullptr if doesn&#39;t exist.
Compound Geometry.
void push_back(const ValueType &val)
Append the data array to hold the new value, resizes if neccesary.
void setNumOutputPorts(const size_t numPorts)
Get/Set the amount of output ports.
std::shared_ptr< Geometry > getOutput(size_t port=0) const
Returns output geometry given port, returns nullptr if doesn&#39;t exist.
void setInput(std::shared_ptr< Geometry > inputGeometry, size_t port=0)
Set the input at the port.
Represents a set of triangles & vertices via an array of Vec3d double vertices & Vec3i integer indice...
void setOutput(std::shared_ptr< Geometry > inputGeometry, const size_t port=0)
Set the output at the port.
void setNumInputPorts(const size_t numPorts)
Get/Set the amount of input ports.