Skip to content

Commit dd3c4f2

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

File tree

2 files changed

+325
-320
lines changed

2 files changed

+325
-320
lines changed

‎src/CollisionAlgorithm/algorithm/InsertionAlgorithm.cpp‎

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

0 commit comments

Comments
 (0)