iMSTK
Interactive Medical Simulation Toolkit
All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Pages
imstkLineMeshToSphereCD.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 "imstkLineMeshToSphereCD.h"
8 #include "imstkCollisionData.h"
9 #include "imstkCollisionUtils.h"
10 #include "imstkParallelUtils.h"
11 #include "imstkSphere.h"
12 #include "imstkLineMesh.h"
13 
14 namespace imstk
15 {
16 LineMeshToSphereCD::LineMeshToSphereCD()
17 {
18  setRequiredInputType<LineMesh>(0);
19  setRequiredInputType<Sphere>(1);
20 }
21 
22 void
24  std::shared_ptr<Geometry> geomA,
25  std::shared_ptr<Geometry> geomB,
26  std::vector<CollisionElement>& elementsA,
27  std::vector<CollisionElement>& elementsB)
28 {
29  std::shared_ptr<LineMesh> lineMesh = std::dynamic_pointer_cast<LineMesh>(geomA);
30  std::shared_ptr<Sphere> sphere = std::dynamic_pointer_cast<Sphere>(geomB);
31 
32  const Vec3d& spherePos = sphere->getPosition();
33  const double sphereRadius = sphere->getRadius();
34 
35  std::shared_ptr<VecDataArray<int, 2>> indicesPtr = lineMesh->getCells();
36  const VecDataArray<int, 2>& indices = *indicesPtr;
37  std::shared_ptr<VecDataArray<double, 3>> verticesPtr = lineMesh->getVertexPositions();
38  const VecDataArray<double, 3>& vertices = *verticesPtr;
39 
41  ParallelUtils::parallelFor(indices.size(), [&](int i)
42  {
43  const Vec2i& cell = indices[i];
44  const Vec3d& x1 = vertices[cell[0]];
45  const Vec3d& x2 = vertices[cell[1]];
46 
47  // This approach does a built in sphere sweep
48  // \todo: Spatial accelerators need to be abstracted
49  const Vec3d centroid = (x1 + x2) / 2.0;
50 
51  // Find the maximal point from centroid for radius
52  const double rSqr1 = (centroid - x1).squaredNorm();
53 
54  const double lineBoundingRadius = std::sqrt(rSqr1);
55 
56  const double distSqr = (centroid - spherePos).squaredNorm();
57  const double rSum = lineBoundingRadius + sphereRadius;
58 
59  if (distSqr < rSum * rSum)
60  {
61  Vec3d lineContactPt;
62  int caseType = -1;
63 
64  lineContactPt = CollisionUtils::closestPointOnSegment(
65  spherePos,
66  x1, x2,
67  caseType);
68 
69  const double sphereDist = (lineContactPt - spherePos).squaredNorm();
70  if (sphereDist <= sphereRadius * sphereRadius)
71  {
72  // Sphere contact with x1
73  if (caseType == 0)
74  {
75  Vec3d contactNormal = (spherePos - lineContactPt);
76  const double dist = contactNormal.norm();
77  const double penetrationDepth = sphereRadius - dist;
78  contactNormal /= dist;
79 
80  // Point contact
82  elemA.ptIndex = cell[0]; // Point on line
83  elemA.dir = -contactNormal; // Direction to resolve point on line
84  elemA.penetrationDepth = penetrationDepth;
85 
87  elemB.pt = spherePos - sphereRadius * contactNormal; // Contact point on sphere
88  elemB.dir = contactNormal; // Direction to resolve point on sphere
89  elemB.penetrationDepth = penetrationDepth;
90 
91  lock.lock();
92  elementsA.push_back(elemA);
93  elementsB.push_back(elemB);
94  lock.unlock();
95  }
96  // Sphere contact with x2
97  else if (caseType == 1)
98  {
99  Vec3d contactNormal = (spherePos - lineContactPt);
100  const double dist = contactNormal.norm();
101  const double penetrationDepth = sphereRadius - dist;
102  contactNormal /= dist;
103 
104  // Point contact
106  elemA.ptIndex = cell[1]; // Point on line
107  elemA.dir = -contactNormal; // Direction to resolve point on line
108  elemA.penetrationDepth = penetrationDepth;
109 
110  PointDirectionElement elemB;
111  elemB.pt = spherePos - sphereRadius * contactNormal; // Contact point on sphere
112  elemB.dir = contactNormal; // Direction to resolve point on sphere
113  elemB.penetrationDepth = penetrationDepth;
114 
115  lock.lock();
116  elementsA.push_back(elemA);
117  elementsB.push_back(elemB);
118  lock.unlock();
119  }
120  // Sphere contact between x1 and x2
121  else if (caseType == 2)
122  {
123  CellIndexElement elemA;
124  elemA.ids[0] = cell[0];
125  elemA.ids[1] = cell[1];
126  elemA.idCount = 2;
127  elemA.parentId = i; // line id
128  elemA.cellType = IMSTK_EDGE;
129 
130  Vec3d contactNormal = (spherePos - lineContactPt);
131  const double dist = contactNormal.norm();
132  const double penetrationDepth = sphereRadius - dist;
133  contactNormal /= dist;
134 
135  PointDirectionElement elemB;
136  elemB.dir = contactNormal; // Direction to resolve sphere
137  elemB.pt = spherePos - sphereRadius * contactNormal; // Contact point on sphere
138  elemB.penetrationDepth = penetrationDepth;
139 
140  lock.lock();
141  elementsA.push_back(elemA);
142  elementsB.push_back(elemB);
143  lock.unlock();
144  }
145  }
146  }
147  }, indices.size() > 500);
148 }
149 } // namespace imstk
The SpinLock class.
Definition: imstkSpinLock.h:20
void unlock()
End a thread-safe region.
Definition: imstkSpinLock.h:53
Vec3d getPosition(DataType type=DataType::PostTransform)
Get the local or global position (post transformed)
Compound Geometry.
void lock()
Start a thread-safe region, where only one thread can execute at a time until a call to the unlock fu...
Definition: imstkSpinLock.h:45
Base class for all volume mesh types.
Definition: imstkLineMesh.h:18
Represents a sphere via its position & radius.
Definition: imstkSphere.h:19
Represents a cell by a single cell id OR by N vertex ids. Which case can be determined by the idCount...
void computeCollisionDataAB(std::shared_ptr< Geometry > geomA, std::shared_ptr< Geometry > geomB, std::vector< CollisionElement > &elementsA, std::vector< CollisionElement > &elementsB) override
Compute collision data for AB simultaneously.
Direclty gives a point-direction contact as its collision data, point given by index.
Direclty gives a point-direction contact as its collision data.