Skip to content

Commit b5b4f24

Browse files
th-skamth-skam
committed
[algorithm] Move function definitions in source file
1 parent 0072225 commit b5b4f24

File tree

2 files changed

+329
-320
lines changed

2 files changed

+329
-320
lines changed

‎src/CollisionAlgorithm/algorithm/InsertionAlgorithm.cpp‎

Lines changed: 321 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,4 +9,325 @@ void registerInsertionAlgorithm(sofa::core::ObjectFactory* factory)
99
"A class implementing a customized needle insertion algorithm")
1010
.add<InsertionAlgorithm>());
1111
}
12+
13+
InsertionAlgorithm::InsertionAlgorithm()
14+
: l_tipGeom(initLink("tipGeom", "Link to the geometry structure of the needle tip.")),
15+
l_surfGeom(
16+
initLink("surfGeom", "Link to the geometry of the surface punctured by the needle.")),
17+
l_shaftGeom(initLink("shaftGeom", "Link to the geometry structure of the needle shaft.")),
18+
l_volGeom(initLink("volGeom",
19+
"Link to the geometry of volume wherein the needle is inserted.")),
20+
d_collisionOutput(initData(&d_collisionOutput, "collisionOutput",
21+
"Detected proximities during puncture.")),
22+
d_insertionOutput(initData(&d_insertionOutput, "insertionOutput",
23+
"Detected proximities during insertion.")),
24+
d_projective(initData(
25+
&d_projective, false, "projective",
26+
"Projection of closest detected proximity back onto the needle tip element.")),
27+
d_enablePuncture(
28+
initData(&d_enablePuncture, true, "enablePuncture", "Enable puncture algorithm.")),
29+
d_enableInsertion(
30+
initData(&d_enableInsertion, true, "enableInsertion", "Enable insertion algorithm.")),
31+
d_enableShaftCollision(initData(&d_enableShaftCollision, true, "enableShaftCollision",
32+
"Enable shaft-surface collision.")),
33+
d_punctureForceThreshold(initData(&d_punctureForceThreshold, -1_sreal,
34+
"punctureForceThreshold",
35+
"Threshold for the force applied to the needle tip. "
36+
"Once exceeded, puncture is initiated.")),
37+
d_tipDistThreshold(initData(&d_tipDistThreshold, -1_sreal, "tipDistThreshold",
38+
"Threshold for the distance advanced by the needle tip since "
39+
"the last proximity detection. Once exceeded, a new "
40+
"proximity pair is added for the needle-volume coupling.")),
41+
m_constraintSolver(nullptr),
42+
m_couplingPts(),
43+
d_drawCollision(initData(&d_drawCollision, false, "drawcollision", "Draw collision.")),
44+
d_drawPoints(initData(&d_drawPoints, false, "drawPoints", "Draw detection outputs.")),
45+
d_drawPointsScale(initData(&d_drawPointsScale, 0.0005, "drawPointsScale",
46+
"Scale the drawing of detection output points."))
47+
{}
48+
49+
void InsertionAlgorithm::init()
50+
{
51+
BaseAlgorithm::init();
52+
53+
if (d_enablePuncture.getValue())
54+
{
55+
this->getContext()->get<ConstraintSolver>(m_constraintSolver);
56+
msg_warning_when(!m_constraintSolver)
57+
<< "No constraint solver found in context. Insertion algorithm is disabled.";
58+
if (d_punctureForceThreshold.getValue() < 0)
59+
{
60+
msg_warning() << d_punctureForceThreshold.getName() +
61+
" parameter not defined or set to negative value." msgendl
62+
<< "Puncture will not function properly; provide a positive value";
63+
d_punctureForceThreshold.setValue(std::numeric_limits<double>::max());
64+
}
65+
}
66+
67+
if (d_enableInsertion.getValue())
68+
{
69+
if (d_tipDistThreshold.getValue() < 0)
70+
{
71+
msg_warning() << d_tipDistThreshold.getName() +
72+
" parameter not defined or set to negative value." msgendl
73+
<< "Needle-volume coupling is disabled; provide a positive value";
74+
d_tipDistThreshold.setValue(std::numeric_limits<double>::max());
75+
}
76+
}
77+
}
78+
79+
void InsertionAlgorithm::draw(const core::visual::VisualParams* vparams)
80+
{
81+
if (!vparams->displayFlags().getShowCollisionModels() && !d_drawCollision.getValue())
82+
return;
83+
vparams->drawTool()->disableLighting();
84+
85+
const AlgorithmOutput& collisionOutput = d_collisionOutput.getValue();
86+
for (const auto& it : collisionOutput)
87+
{
88+
vparams->drawTool()->drawLine(it.first->getPosition(), it.second->getPosition(),
89+
type::RGBAColor(0, 1, 0, 1));
90+
}
91+
92+
const AlgorithmOutput& insertionOutput = d_insertionOutput.getValue();
93+
for (const auto& it : insertionOutput)
94+
{
95+
vparams->drawTool()->drawSphere(it.first->getPosition(), d_drawPointsScale.getValue(),
96+
type::RGBAColor(1, 0, 1, 0.9));
97+
vparams->drawTool()->drawSphere(it.second->getPosition(), d_drawPointsScale.getValue(),
98+
type::RGBAColor(0, 0, 1, 0.9));
99+
vparams->drawTool()->drawLine(it.first->getPosition(), it.second->getPosition(),
100+
type::RGBAColor(1, 1, 0, 1));
101+
}
102+
}
103+
104+
void InsertionAlgorithm::doDetection()
105+
{
106+
auto& collisionOutput = *d_collisionOutput.beginEdit();
107+
auto& insertionOutput = *d_insertionOutput.beginEdit();
108+
109+
insertionOutput.clear();
110+
collisionOutput.clear();
111+
112+
if (m_couplingPts.empty() && l_surfGeom)
113+
{
114+
// Operations on surface geometry
115+
sofa::helper::AdvancedTimer::stepBegin("Puncture detection - "+this->getName());
116+
117+
auto findClosestProxOnSurf =
118+
Operations::FindClosestProximity::Operation::get(l_surfGeom);
119+
auto projectOnSurf = Operations::Project::Operation::get(l_surfGeom);
120+
121+
// Puncture sequence
122+
if (d_enablePuncture.getValue() && l_tipGeom)
123+
{
124+
auto createTipProximity =
125+
Operations::CreateCenterProximity::Operation::get(l_tipGeom->getTypeInfo());
126+
auto projectOnTip = Operations::Project::Operation::get(l_tipGeom);
127+
128+
const SReal punctureForceThreshold = d_punctureForceThreshold.getValue();
129+
for (auto itTip = l_tipGeom->begin(); itTip != l_tipGeom->end(); itTip++)
130+
{
131+
BaseProximity::SPtr tipProx = createTipProximity(itTip->element());
132+
if (!tipProx) continue;
133+
const BaseProximity::SPtr surfProx = findClosestProxOnSurf(
134+
tipProx, l_surfGeom.get(), projectOnSurf, getFilterFunc());
135+
if (surfProx)
136+
{
137+
surfProx->normalize();
138+
139+
// Check whether puncture is happening - if so, create coupling point ...
140+
if (m_constraintSolver)
141+
{
142+
const MechStateTipType::SPtr mstate =
143+
l_tipGeom->getContext()->get<MechStateTipType>();
144+
const auto& lambda =
145+
m_constraintSolver->getLambda()[mstate.get()].read()->getValue();
146+
SReal norm{0_sreal};
147+
for (const auto& l : lambda)
148+
{
149+
norm += l.norm();
150+
}
151+
if (norm > punctureForceThreshold)
152+
{
153+
m_couplingPts.push_back(surfProx);
154+
continue;
155+
}
156+
}
157+
158+
// ... if not, create a proximity pair for the tip-surface collision
159+
collisionOutput.add(tipProx, surfProx);
160+
}
161+
}
162+
}
163+
sofa::helper::AdvancedTimer::stepEnd("Puncture detection - "+this->getName());
164+
165+
// Shaft collision sequence - Disable if coupling points have been added
166+
sofa::helper::AdvancedTimer::stepBegin("Shaft collision - "+this->getName());
167+
if (d_enableShaftCollision.getValue() && m_couplingPts.empty() && l_shaftGeom)
168+
{
169+
auto createShaftProximity =
170+
Operations::CreateCenterProximity::Operation::get(l_shaftGeom->getTypeInfo());
171+
auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom);
172+
for (auto itShaft = l_shaftGeom->begin(); itShaft != l_shaftGeom->end(); itShaft++)
173+
{
174+
BaseProximity::SPtr shaftProx = createShaftProximity(itShaft->element());
175+
if (!shaftProx) continue;
176+
const BaseProximity::SPtr surfProx = findClosestProxOnSurf(
177+
shaftProx, l_surfGeom.get(), projectOnSurf, getFilterFunc());
178+
if (surfProx)
179+
{
180+
surfProx->normalize();
181+
182+
if (d_projective.getValue())
183+
{
184+
//shaftProx =
185+
// projectOnShaft(surfProx->getPosition(), itShaft->element()).prox;
186+
//if (!shaftProx) continue;
187+
//shaftProx->normalize();
188+
// Experimental - This enables projection anywhere on the edge
189+
auto findClosestProxOnShaft =
190+
Operations::FindClosestProximity::Operation::get(l_shaftGeom);
191+
shaftProx = findClosestProxOnShaft(surfProx, l_shaftGeom,
192+
projectOnShaft, getFilterFunc());
193+
if (!shaftProx) continue;
194+
shaftProx->normalize();
195+
}
196+
collisionOutput.add(shaftProx, surfProx);
197+
}
198+
}
199+
}
200+
sofa::helper::AdvancedTimer::stepEnd("Shaft collision - "+this->getName());
201+
}
202+
else
203+
{
204+
// Insertion sequence
205+
sofa::helper::AdvancedTimer::stepBegin("Needle insertion - " + this->getName());
206+
if (!d_enableInsertion.getValue() || !l_tipGeom || !l_volGeom || !l_shaftGeom) return;
207+
208+
ElementIterator::SPtr itTip = l_tipGeom->begin();
209+
auto createTipProximity =
210+
Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo());
211+
const BaseProximity::SPtr tipProx = createTipProximity(itTip->element());
212+
if (!tipProx) return;
213+
214+
// Remove coupling points that are ahead of the tip in the insertion direction
215+
ElementIterator::SPtr itShaft = l_shaftGeom->begin(l_shaftGeom->getSize() - 2);
216+
auto prunePointsAheadOfTip =
217+
Operations::Needle::PrunePointsAheadOfTip::get(itShaft->getTypeInfo());
218+
prunePointsAheadOfTip(m_couplingPts, itShaft->element());
219+
220+
if (m_couplingPts.empty()) return;
221+
222+
type::Vec3 lastCP = m_couplingPts.back()->getPosition();
223+
const SReal tipDistThreshold = this->d_tipDistThreshold.getValue();
224+
225+
// Vector from tip to last coupling point; used for distance and directional checks
226+
const type::Vec3 tipToLastCP = lastCP - tipProx->getPosition();
227+
228+
// Only add a new coupling point if the needle tip has advanced far enough
229+
if (tipToLastCP.norm() > tipDistThreshold)
230+
{
231+
// Prepare the operations before entering the loop
232+
auto createShaftProximity =
233+
Operations::CreateCenterProximity::Operation::get(l_shaftGeom->getTypeInfo());
234+
auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom);
235+
auto findClosestProxOnVol =
236+
Operations::FindClosestProximity::Operation::get(l_volGeom);
237+
auto projectOnVol = Operations::Project::Operation::get(l_volGeom);
238+
auto containsPointInVol =
239+
Operations::ContainsPointInProximity::Operation::get(l_volGeom);
240+
241+
// Iterate over shaft segments to find which one contains the next candidate CP
242+
for (auto itShaft = l_shaftGeom->begin(); itShaft != l_shaftGeom->end(); itShaft++)
243+
{
244+
BaseProximity::SPtr shaftProx = createShaftProximity(itShaft->element());
245+
if (!shaftProx) continue;
246+
247+
const EdgeProximity::SPtr edgeProx =
248+
dynamic_pointer_cast<EdgeProximity>(shaftProx);
249+
if (!edgeProx) continue;
250+
251+
const type::Vec3 p0 = edgeProx->element()->getP0()->getPosition();
252+
const type::Vec3 p1 = edgeProx->element()->getP1()->getPosition();
253+
const type::Vec3 shaftEdgeDir = (p1 - p0).normalized();
254+
const type::Vec3 lastCPToP1 = p1 - lastCP;
255+
256+
// Skip if last CP lies after edge end point
257+
if (dot(shaftEdgeDir, lastCPToP1) < 0_sreal) continue;
258+
259+
const int numCPs = floor(lastCPToP1.norm() / tipDistThreshold);
260+
261+
for (int idCP = 0; idCP < numCPs; idCP++)
262+
{
263+
// Candidate coupling point along shaft segment
264+
const type::Vec3 candidateCP = lastCP + tipDistThreshold * shaftEdgeDir;
265+
266+
// Project candidate CP onto the edge element and compute scalar coordinate
267+
// along segment
268+
const SReal edgeSegmentLength = (p1 - p0).norm();
269+
const type::Vec3 p0ToCandidateCP = candidateCP - p0;
270+
const SReal projPtOnEdge = dot(p0ToCandidateCP, shaftEdgeDir);
271+
272+
// Skip if candidate CP is outside current edge segment
273+
if (projPtOnEdge < 0_sreal || projPtOnEdge > edgeSegmentLength) break;
274+
275+
// Project candidate CP onto shaft geometry ...
276+
shaftProx = projectOnShaft(candidateCP, itShaft->element()).prox;
277+
if (!shaftProx) continue;
278+
279+
// ... then find nearest volume proximity
280+
const BaseProximity::SPtr volProx = findClosestProxOnVol(
281+
shaftProx, l_volGeom.get(), projectOnVol, getFilterFunc());
282+
if (!volProx) continue;
283+
284+
// Proximity can be detected before the tip enters the tetra (e.g. near a
285+
// boundary face) Only accept proximities if the tip is inside the tetra
286+
// during insertion
287+
if (containsPointInVol(shaftProx->getPosition(), volProx))
288+
{
289+
volProx->normalize();
290+
m_couplingPts.push_back(volProx);
291+
lastCP = volProx->getPosition();
292+
}
293+
}
294+
}
295+
}
296+
else // Don't bother with removing the point that was just added
297+
{
298+
// Remove coupling points that are ahead of the tip in the insertion direction
299+
ElementIterator::SPtr itShaft = l_shaftGeom->begin(l_shaftGeom->getSize() - 2);
300+
auto prunePointsAheadOfTip =
301+
Operations::Needle::PrunePointsAheadOfTip::get(itShaft->getTypeInfo());
302+
prunePointsAheadOfTip(m_couplingPts, itShaft->element());
303+
}
304+
sofa::helper::AdvancedTimer::stepEnd("Needle insertion - " + this->getName());
305+
}
306+
307+
sofa::helper::AdvancedTimer::stepBegin("Reproject coupling points - "+this->getName());
308+
if (d_enableInsertion.getValue() && !m_couplingPts.empty() && l_shaftGeom)
309+
{
310+
// Reprojection on shaft geometry sequence
311+
auto findClosestProxOnShaft =
312+
Operations::FindClosestProximity::Operation::get(l_shaftGeom);
313+
auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom);
314+
for (const auto& cp : m_couplingPts)
315+
{
316+
const BaseProximity::SPtr shaftProx = findClosestProxOnShaft(
317+
cp, l_shaftGeom.get(), projectOnShaft, getFilterFunc());
318+
if (!shaftProx) continue;
319+
shaftProx->normalize();
320+
insertionOutput.add(shaftProx, cp);
321+
}
322+
// This is a final-frontier check: If there are coupling points stored, but the
323+
// findClosestProxOnShaf operation yields no proximities on the shaft, it could be
324+
// because the needle has exited abruptly. Thus, we clear the coupling points.
325+
if (insertionOutput.size() == 0) m_couplingPts.clear();
326+
}
327+
sofa::helper::AdvancedTimer::stepEnd("Reproject coupling points - "+this->getName());
328+
329+
d_collisionOutput.endEdit();
330+
d_insertionOutput.endEdit();
331+
}
332+
12333
} // namespace sofa::collisionalgorithm

0 commit comments

Comments
 (0)