iMSTK
Interactive Medical Simulation Toolkit
imstkSurfaceMeshToSurfaceMeshCD.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 "imstkSurfaceMeshToSurfaceMeshCD.h"
8 #include "imstkCollisionUtils.h"
9 #include "imstkSurfaceMesh.h"
10 #include "imstkGeometryUtilities.h"
11 
12 struct EdgePair
13 {
14  EdgePair(uint32_t a1, uint32_t a2, uint32_t b1, uint32_t b2)
15  {
16  edgeA[0] = a1;
17  edgeA[1] = a2;
18  edgeB[0] = b1;
19  edgeB[1] = b2;
20 
21  edgeAId = getIdA();
22  edgeBId = getIdB();
23  }
24 
29  bool operator==(const EdgePair& other) const
30  {
31  return (edgeAId == other.edgeAId && edgeBId == other.edgeBId)
32  || (edgeAId == other.edgeBId && edgeBId == other.edgeAId);
33  }
34 
35  // These functions return a unique int for an edge, order doesn't matter
36  // ie: f(vertexId1, vertexId2)=f(vertexId2, vertexId1)
37  const uint32_t getIdA() const
38  {
39  const uint32_t max = std::max(edgeA[0], edgeA[1]);
40  const uint32_t min = std::min(edgeA[0], edgeA[1]);
41  return max * (max + 1) / 2 + min;
42  }
43 
44  const uint32_t getIdB() const
45  {
46  const uint32_t max = std::max(edgeB[0], edgeB[1]);
47  const uint32_t min = std::min(edgeB[0], edgeB[1]);
48  return max * (max + 1) / 2 + min;
49  }
50 
51  uint32_t edgeA[2];
52  uint32_t edgeAId;
53  uint32_t edgeB[2];
54  uint32_t edgeBId;
55 };
56 
57 namespace std
58 {
59 template<>
60 struct hash<EdgePair>
61 {
62  // EdgePair has 4 uints to hash, they bound the same range, 0 to max vertices of a mesh
63  // A complete unique hash split into 4, would limit us to 256 max vertices so we will have
64  // collisions but they will be unlikely given small portions of the mesh are in contact at
65  // any one time
66  std::size_t operator()(const EdgePair& k) const
67  {
68  // Shift by 8 each time, there will be overlap every 256 ints
69  //return ((k.edgeA[0] ^ (k.edgeA[1] << 8)) ^ (k.edgeB[0] << 16)) ^ (k.edgeB[1] << 24);
70 
71  // The edge ids are more compact since f(1,0)=f(0,1) there are fewer permutations,
72  // This should allow up to ~360 max vertices..., not that much better
73  return (k.edgeAId ^ (k.edgeBId << 16));
74  }
75 };
76 } // namespace std
77 
78 namespace imstk
79 {
80 SurfaceMeshToSurfaceMeshCD::SurfaceMeshToSurfaceMeshCD()
81 {
82  setRequiredInputType<SurfaceMesh>(0);
83  setRequiredInputType<SurfaceMesh>(1);
84 
85  // By default generate contact data for both sides
86  setGenerateCD(true, true);
87 }
88 
89 void
90 SurfaceMeshToSurfaceMeshCD::computeCollisionDataAB(
91  std::shared_ptr<Geometry> geomA,
92  std::shared_ptr<Geometry> geomB,
93  std::vector<CollisionElement>& elementsA,
94  std::vector<CollisionElement>& elementsB)
95 {
96  std::shared_ptr<SurfaceMesh> surfMeshA = std::dynamic_pointer_cast<SurfaceMesh>(geomA);
97  std::shared_ptr<VecDataArray<double, 3>> verticesAPtr = surfMeshA->getVertexPositions();
98  VecDataArray<double, 3>& verticesA = *verticesAPtr;
99  std::shared_ptr<VecDataArray<int, 3>> indicesAPtr = surfMeshA->getCells();
100  const VecDataArray<int, 3>& indicesA = *indicesAPtr;
101 
102  std::shared_ptr<SurfaceMesh> surfMeshB = std::dynamic_pointer_cast<SurfaceMesh>(geomB);
103  std::shared_ptr<VecDataArray<double, 3>> verticesBPtr = surfMeshB->getVertexPositions();
104  VecDataArray<double, 3>& verticesB = *verticesBPtr;
105  std::shared_ptr<VecDataArray<int, 3>> indicesBPtr = surfMeshB->getCells();
106  const VecDataArray<int, 3>& indicesB = *indicesBPtr;
107 
108  std::unordered_set<EdgePair> edges;
109  for (int i = 0; i < indicesA.size(); i++)
110  {
111  const Vec3i& cellA = indicesA[i];
112  for (int j = 0; j < indicesB.size(); j++)
113  {
114  const Vec3i& cellB = indicesB[j];
115 
116  // vtContact needs to be checked both ways but eeContact is symmetric
117  std::pair<Vec2i, Vec2i> eeContact;
118  std::pair<int, Vec3i> vtContact;
119  std::pair<Vec3i, int> tvContact;
120  const int contactType = CollisionUtils::triangleToTriangle(cellA, cellB,
121  verticesA[cellA[0]], verticesA[cellA[1]], verticesA[cellA[2]],
122  verticesB[cellB[0]], verticesB[cellB[1]], verticesB[cellB[2]],
123  eeContact, vtContact, tvContact);
124 
125  // If you want to visualize the cells in contact
126  // report triangle vs triangle instead
127  /* CellIndexElement elemB;
128  elemB.idCount = 3;
129  elemB.cellType = IMSTK_TRIANGLE;
130  elemB.ids[0] = cellB[0];
131  elemB.ids[1] = cellB[1];
132  elemB.ids[2] = cellB[2];
133  CellIndexElement elemA;
134  elemA.idCount = 3;
135  elemA.cellType = IMSTK_TRIANGLE;
136  elemA.ids[0] = cellA[0];
137  elemA.ids[1] = cellA[1];
138  elemA.ids[2] = cellA[2];
139  elementsA.unsafeAppend(elemA);
140  elementsB.unsafeAppend(elemB);*/
141 
142  // Type 1, vertex-triangle contact
143  if (contactType == 1)
144  {
145  CellIndexElement elemA;
146  elemA.idCount = 1;
147  elemA.cellType = IMSTK_VERTEX;
148  elemA.ids[0] = vtContact.first;
149 
150  CellIndexElement elemB;
151  elemB.idCount = 3;
152  elemB.cellType = IMSTK_TRIANGLE;
153  elemB.ids[0] = vtContact.second[0];
154  elemB.ids[1] = vtContact.second[1];
155  elemB.ids[2] = vtContact.second[2];
156  elemA.parentId = j; // Triangle id
157 
158  elementsA.push_back(elemA);
159  elementsB.push_back(elemB);
160  }
161  // Type 0, edge-edge contact
162  else if (contactType == 0)
163  {
164  // Create an edge pair and hash it to see if we already have this contact from
165  // another triangle
166  const EdgePair edgePair = {
167  static_cast<uint32_t>(eeContact.first[0]),
168  static_cast<uint32_t>(eeContact.first[1]),
169  static_cast<uint32_t>(eeContact.second[0]),
170  static_cast<uint32_t>(eeContact.second[1]) };
171  if (edges.count(edgePair) == 0)
172  {
173  CellIndexElement elemA;
174  elemA.idCount = 2;
175  elemA.cellType = IMSTK_EDGE;
176  elemA.ids[0] = eeContact.first[0];
177  elemA.ids[1] = eeContact.first[1];
178  elemA.parentId = i; // Triangle id
179 
180  CellIndexElement elemB;
181  elemB.idCount = 2;
182  elemB.cellType = IMSTK_EDGE;
183  elemB.ids[0] = eeContact.second[0];
184  elemB.ids[1] = eeContact.second[1];
185  elemA.parentId = j; // Triangle id
186 
187  elementsA.push_back(elemA);
188  elementsB.push_back(elemB);
189  edges.insert(edgePair);
190  }
191  }
192  // Type 3, triangle-vertex contact
193  else if (contactType == 2)
194  {
195  CellIndexElement elemA;
196  elemA.idCount = 3;
197  elemA.cellType = IMSTK_TRIANGLE;
198  elemA.ids[0] = tvContact.first[0];
199  elemA.ids[1] = tvContact.first[1];
200  elemA.ids[2] = tvContact.first[2];
201  elemA.parentId = i; // Triangle id
202 
203  CellIndexElement elemB;
204  elemB.idCount = 1;
205  elemB.cellType = IMSTK_VERTEX;
206  elemB.ids[0] = tvContact.second;
207 
208  elementsA.push_back(elemA);
209  elementsB.push_back(elemB);
210  }
211  //else
212  //{
213  // // This case is hit in one edge case
214  // LOG(WARNING) << "Contact without intersection!";
215  //}
216  }
217  }
218 }
219 } // namespace imstk
imstk::EdgePair
Hash together a pair of edges.
Definition: imstkClosedSurfaceMeshToMeshCD.cpp:20
imstk
Compound Geometry.
Definition: OctreeDebugModel.cpp:10
hash
Returns a hash value for a PointEntry.
imstk::PointSet::getVertexPositions
std::shared_ptr< VecDataArray< double, 3 > > getVertexPositions(DataType type=DataType::PostTransform) const
Returns the vector of current positions of the mesh vertices.
Definition: imstkPointSet.cpp:110
imstk::SurfaceMesh
Represents a set of triangles & vertices via an array of Vec3d double vertices & Vec3i integer indice...
Definition: imstkSurfaceMesh.h:124
imstk::CellIndexElement
Represents a cell by a single cell id OR by N vertex ids. Which case can be determined by the idCount...
Definition: imstkCollisionData.h:66
EdgePair::operator==
bool operator==(const EdgePair &other) const
Reversible edges are equivalent, reversible vertices in the edges are equivalent as well EdgePair(0...
Definition: imstkSurfaceMeshToSurfaceMeshCD.cpp:29
Generated by   doxygen 1.8.13