7 #include "imstkLineMeshToSphereCD.h" 8 #include "imstkCollisionData.h" 9 #include "imstkCollisionUtils.h" 10 #include "imstkParallelUtils.h" 11 #include "imstkSphere.h" 12 #include "imstkLineMesh.h" 16 LineMeshToSphereCD::LineMeshToSphereCD()
18 setRequiredInputType<LineMesh>(0);
19 setRequiredInputType<Sphere>(1);
24 std::shared_ptr<Geometry> geomA,
25 std::shared_ptr<Geometry> geomB,
26 std::vector<CollisionElement>& elementsA,
27 std::vector<CollisionElement>& elementsB)
29 std::shared_ptr<LineMesh> lineMesh = std::dynamic_pointer_cast<
LineMesh>(geomA);
30 std::shared_ptr<Sphere> sphere = std::dynamic_pointer_cast<
Sphere>(geomB);
33 const double sphereRadius = sphere->getRadius();
35 std::shared_ptr<VecDataArray<int, 2>> indicesPtr = lineMesh->getCells();
37 std::shared_ptr<VecDataArray<double, 3>> verticesPtr = lineMesh->getVertexPositions();
41 ParallelUtils::parallelFor(indices.size(), [&](
int i)
43 const Vec2i& cell = indices[i];
44 const Vec3d& x1 = vertices[cell[0]];
45 const Vec3d& x2 = vertices[cell[1]];
49 const Vec3d centroid = (x1 + x2) / 2.0;
52 const double rSqr1 = (centroid - x1).squaredNorm();
54 const double lineBoundingRadius = std::sqrt(rSqr1);
56 const double distSqr = (centroid - spherePos).squaredNorm();
57 const double rSum = lineBoundingRadius + sphereRadius;
59 if (distSqr < rSum * rSum)
64 lineContactPt = CollisionUtils::closestPointOnSegment(
69 const double sphereDist = (lineContactPt - spherePos).squaredNorm();
70 if (sphereDist <= sphereRadius * sphereRadius)
75 Vec3d contactNormal = (spherePos - lineContactPt);
76 const double dist = contactNormal.norm();
77 const double penetrationDepth = sphereRadius - dist;
78 contactNormal /= dist;
82 elemA.ptIndex = cell[0];
83 elemA.dir = -contactNormal;
84 elemA.penetrationDepth = penetrationDepth;
87 elemB.pt = spherePos - sphereRadius * contactNormal;
88 elemB.dir = contactNormal;
89 elemB.penetrationDepth = penetrationDepth;
92 elementsA.push_back(elemA);
93 elementsB.push_back(elemB);
97 else if (caseType == 1)
99 Vec3d contactNormal = (spherePos - lineContactPt);
100 const double dist = contactNormal.norm();
101 const double penetrationDepth = sphereRadius - dist;
102 contactNormal /= dist;
106 elemA.ptIndex = cell[1];
107 elemA.dir = -contactNormal;
108 elemA.penetrationDepth = penetrationDepth;
111 elemB.pt = spherePos - sphereRadius * contactNormal;
112 elemB.dir = contactNormal;
113 elemB.penetrationDepth = penetrationDepth;
116 elementsA.push_back(elemA);
117 elementsB.push_back(elemB);
121 else if (caseType == 2)
124 elemA.ids[0] = cell[0];
125 elemA.ids[1] = cell[1];
128 elemA.cellType = IMSTK_EDGE;
130 Vec3d contactNormal = (spherePos - lineContactPt);
131 const double dist = contactNormal.norm();
132 const double penetrationDepth = sphereRadius - dist;
133 contactNormal /= dist;
136 elemB.dir = contactNormal;
137 elemB.pt = spherePos - sphereRadius * contactNormal;
138 elemB.penetrationDepth = penetrationDepth;
141 elementsA.push_back(elemA);
142 elementsB.push_back(elemB);
147 }, indices.size() > 500);
void unlock()
End a thread-safe region.
Vec3d getPosition(DataType type=DataType::PostTransform)
Get the local or global position (post transformed)
void lock()
Start a thread-safe region, where only one thread can execute at a time until a call to the unlock fu...
Base class for all volume mesh types.
Represents a sphere via its position & radius.
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.