Skip to content

Commit 3c9cd8a

Browse files
th-skamth-skam
committed
[algorithm] Moved insertion code to separate functions
1 parent dd81a05 commit 3c9cd8a

File tree

2 files changed

+128
-114
lines changed

2 files changed

+128
-114
lines changed

‎src/CollisionAlgorithm/algorithm/InsertionAlgorithm.cpp‎

Lines changed: 126 additions & 114 deletions
Original file line numberDiff line numberDiff line change
@@ -126,124 +126,12 @@ void InsertionAlgorithm::doDetection()
126126
{
127127
// Insertion sequence
128128
sofa::helper::AdvancedTimer::stepBegin("Needle insertion - " + this->getName());
129-
if (!d_enableInsertion.getValue() || !l_tipGeom || !l_volGeom || !l_shaftGeom) return;
130-
131-
ElementIterator::SPtr itTip = l_tipGeom->begin();
132-
auto createTipProximity =
133-
Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo());
134-
const BaseProximity::SPtr tipProx = createTipProximity(itTip->element());
135-
if (!tipProx) return;
136-
137-
// Remove coupling points that are ahead of the tip in the insertion direction
138-
ElementIterator::SPtr itShaft = l_shaftGeom->begin(l_shaftGeom->getSize() - 2);
139-
auto prunePointsAheadOfTip =
140-
Operations::Needle::PrunePointsAheadOfTip::get(itShaft->getTypeInfo());
141-
prunePointsAheadOfTip(m_couplingPts, itShaft->element());
142-
143-
if (m_couplingPts.empty()) return;
144-
145-
type::Vec3 lastCP = m_couplingPts.back()->getPosition();
146-
const SReal tipDistThreshold = this->d_tipDistThreshold.getValue();
147-
148-
// Vector from tip to last coupling point; used for distance and directional checks
149-
const type::Vec3 tipToLastCP = lastCP - tipProx->getPosition();
150-
151-
// Only add a new coupling point if the needle tip has advanced far enough
152-
if (tipToLastCP.norm() > tipDistThreshold)
153-
{
154-
// Prepare the operations before entering the loop
155-
auto createShaftProximity =
156-
Operations::CreateCenterProximity::Operation::get(l_shaftGeom->getTypeInfo());
157-
auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom);
158-
auto findClosestProxOnVol = Operations::FindClosestProximity::Operation::get(l_volGeom);
159-
auto projectOnVol = Operations::Project::Operation::get(l_volGeom);
160-
auto containsPointInVol =
161-
Operations::ContainsPointInProximity::Operation::get(l_volGeom);
162-
163-
// Iterate over shaft segments to find which one contains the next candidate CP
164-
for (auto itShaft = l_shaftGeom->begin(); itShaft != l_shaftGeom->end(); itShaft++)
165-
{
166-
BaseProximity::SPtr shaftProx = createShaftProximity(itShaft->element());
167-
if (!shaftProx) continue;
168-
169-
const EdgeProximity::SPtr edgeProx = dynamic_pointer_cast<EdgeProximity>(shaftProx);
170-
if (!edgeProx) continue;
171-
172-
const type::Vec3 p0 = edgeProx->element()->getP0()->getPosition();
173-
const type::Vec3 p1 = edgeProx->element()->getP1()->getPosition();
174-
const type::Vec3 shaftEdgeDir = (p1 - p0).normalized();
175-
const type::Vec3 lastCPToP1 = p1 - lastCP;
176-
177-
// Skip if last CP lies after edge end point
178-
if (dot(shaftEdgeDir, lastCPToP1) < 0_sreal) continue;
179-
180-
const int numCPs = floor(lastCPToP1.norm() / tipDistThreshold);
181-
182-
for (int idCP = 0; idCP < numCPs; idCP++)
183-
{
184-
// Candidate coupling point along shaft segment
185-
const type::Vec3 candidateCP = lastCP + tipDistThreshold * shaftEdgeDir;
186-
187-
// Project candidate CP onto the edge element and compute scalar coordinate
188-
// along segment
189-
const SReal edgeSegmentLength = (p1 - p0).norm();
190-
const type::Vec3 p0ToCandidateCP = candidateCP - p0;
191-
const SReal projPtOnEdge = dot(p0ToCandidateCP, shaftEdgeDir);
192-
193-
// Skip if candidate CP is outside current edge segment
194-
if (projPtOnEdge < 0_sreal || projPtOnEdge > edgeSegmentLength) break;
195-
196-
// Project candidate CP onto shaft geometry ...
197-
shaftProx = projectOnShaft(candidateCP, itShaft->element()).prox;
198-
if (!shaftProx) continue;
199-
200-
// ... then find nearest volume proximity
201-
const BaseProximity::SPtr volProx = findClosestProxOnVol(
202-
shaftProx, l_volGeom.get(), projectOnVol, getFilterFunc());
203-
if (!volProx) continue;
204-
205-
// Proximity can be detected before the tip enters the tetra (e.g. near a
206-
// boundary face) Only accept proximities if the tip is inside the tetra
207-
// during insertion
208-
if (containsPointInVol(shaftProx->getPosition(), volProx))
209-
{
210-
volProx->normalize();
211-
m_couplingPts.push_back(volProx);
212-
lastCP = volProx->getPosition();
213-
}
214-
}
215-
}
216-
}
217-
else // Don't bother with removing the point that was just added
218-
{
219-
// Remove coupling points that are ahead of the tip in the insertion direction
220-
ElementIterator::SPtr itShaft = l_shaftGeom->begin(l_shaftGeom->getSize() - 2);
221-
auto prunePointsAheadOfTip =
222-
Operations::Needle::PrunePointsAheadOfTip::get(itShaft->getTypeInfo());
223-
prunePointsAheadOfTip(m_couplingPts, itShaft->element());
224-
}
129+
if (d_enableInsertion.getValue()) insertionPhase();
225130
sofa::helper::AdvancedTimer::stepEnd("Needle insertion - " + this->getName());
226131
}
227132

228133
sofa::helper::AdvancedTimer::stepBegin("Reproject coupling points - " + this->getName());
229-
if (d_enableInsertion.getValue() && !m_couplingPts.empty() && l_shaftGeom)
230-
{
231-
// Reprojection on shaft geometry sequence
232-
auto findClosestProxOnShaft = Operations::FindClosestProximity::Operation::get(l_shaftGeom);
233-
auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom);
234-
for (const auto& cp : m_couplingPts)
235-
{
236-
const BaseProximity::SPtr shaftProx =
237-
findClosestProxOnShaft(cp, l_shaftGeom.get(), projectOnShaft, getFilterFunc());
238-
if (!shaftProx) continue;
239-
shaftProx->normalize();
240-
insertionOutput.add(shaftProx, cp);
241-
}
242-
// This is a final-frontier check: If there are coupling points stored, but the
243-
// findClosestProxOnShaf operation yields no proximities on the shaft, it could be
244-
// because the needle has exited abruptly. Thus, we clear the coupling points.
245-
if (insertionOutput.size() == 0) m_couplingPts.clear();
246-
}
134+
if (d_enableInsertion.getValue()) insertionOutput = reprojectCouplingPoints();
247135
sofa::helper::AdvancedTimer::stepEnd("Reproject coupling points - " + this->getName());
248136

249137
d_collisionOutput.endEdit();
@@ -343,4 +231,128 @@ InsertionAlgorithm::AlgorithmOutput InsertionAlgorithm::shaftCollisionPhase()
343231
return shaftCollisionOutput;
344232
}
345233

234+
void InsertionAlgorithm::insertionPhase()
235+
{
236+
if (!l_tipGeom || !l_volGeom || !l_shaftGeom) return;
237+
238+
ElementIterator::SPtr itTip = l_tipGeom->begin();
239+
auto createTipProximity =
240+
Operations::CreateCenterProximity::Operation::get(itTip->getTypeInfo());
241+
const BaseProximity::SPtr tipProx = createTipProximity(itTip->element());
242+
if (!tipProx) return;
243+
244+
// Remove coupling points that are ahead of the tip in the insertion direction
245+
ElementIterator::SPtr itShaft = l_shaftGeom->begin(l_shaftGeom->getSize() - 2);
246+
auto prunePointsAheadOfTip =
247+
Operations::Needle::PrunePointsAheadOfTip::get(itShaft->getTypeInfo());
248+
prunePointsAheadOfTip(m_couplingPts, itShaft->element());
249+
250+
if (m_couplingPts.empty()) return;
251+
252+
type::Vec3 lastCP = m_couplingPts.back()->getPosition();
253+
const SReal tipDistThreshold = this->d_tipDistThreshold.getValue();
254+
255+
// Vector from tip to last coupling point; used for distance and directional checks
256+
const type::Vec3 tipToLastCP = lastCP - tipProx->getPosition();
257+
258+
// Only add a new coupling point if the needle tip has advanced far enough
259+
if (tipToLastCP.norm() > tipDistThreshold)
260+
{
261+
// Prepare the operations before entering the loop
262+
auto createShaftProximity =
263+
Operations::CreateCenterProximity::Operation::get(l_shaftGeom->getTypeInfo());
264+
auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom);
265+
auto findClosestProxOnVol = Operations::FindClosestProximity::Operation::get(l_volGeom);
266+
auto projectOnVol = Operations::Project::Operation::get(l_volGeom);
267+
auto containsPointInVol = Operations::ContainsPointInProximity::Operation::get(l_volGeom);
268+
269+
// Iterate over shaft segments to find which one contains the next candidate CP
270+
for (auto itShaft = l_shaftGeom->begin(); itShaft != l_shaftGeom->end(); itShaft++)
271+
{
272+
BaseProximity::SPtr shaftProx = createShaftProximity(itShaft->element());
273+
if (!shaftProx) continue;
274+
275+
const EdgeProximity::SPtr edgeProx = dynamic_pointer_cast<EdgeProximity>(shaftProx);
276+
if (!edgeProx) continue;
277+
278+
const type::Vec3 p0 = edgeProx->element()->getP0()->getPosition();
279+
const type::Vec3 p1 = edgeProx->element()->getP1()->getPosition();
280+
const type::Vec3 shaftEdgeDir = (p1 - p0).normalized();
281+
const type::Vec3 lastCPToP1 = p1 - lastCP;
282+
283+
// Skip if last CP lies after edge end point
284+
if (dot(shaftEdgeDir, lastCPToP1) < 0_sreal) continue;
285+
286+
const int numCPs = floor(lastCPToP1.norm() / tipDistThreshold);
287+
288+
for (int idCP = 0; idCP < numCPs; idCP++)
289+
{
290+
// Candidate coupling point along shaft segment
291+
const type::Vec3 candidateCP = lastCP + tipDistThreshold * shaftEdgeDir;
292+
293+
// Project candidate CP onto the edge element and compute scalar coordinate
294+
// along segment
295+
const SReal edgeSegmentLength = (p1 - p0).norm();
296+
const type::Vec3 p0ToCandidateCP = candidateCP - p0;
297+
const SReal projPtOnEdge = dot(p0ToCandidateCP, shaftEdgeDir);
298+
299+
// Skip if candidate CP is outside current edge segment
300+
if (projPtOnEdge < 0_sreal || projPtOnEdge > edgeSegmentLength) break;
301+
302+
// Project candidate CP onto shaft geometry ...
303+
shaftProx = projectOnShaft(candidateCP, itShaft->element()).prox;
304+
if (!shaftProx) continue;
305+
306+
// ... then find nearest volume proximity
307+
const BaseProximity::SPtr volProx =
308+
findClosestProxOnVol(shaftProx, l_volGeom.get(), projectOnVol, getFilterFunc());
309+
if (!volProx) continue;
310+
311+
// Proximity can be detected before the tip enters the tetra (e.g. near a
312+
// boundary face) Only accept proximities if the tip is inside the tetra
313+
// during insertion
314+
if (containsPointInVol(shaftProx->getPosition(), volProx))
315+
{
316+
volProx->normalize();
317+
m_couplingPts.push_back(volProx);
318+
lastCP = volProx->getPosition();
319+
}
320+
}
321+
}
322+
}
323+
else // Don't bother with removing the point that was just added
324+
{
325+
// Remove coupling points that are ahead of the tip in the insertion direction
326+
ElementIterator::SPtr itShaft = l_shaftGeom->begin(l_shaftGeom->getSize() - 2);
327+
auto prunePointsAheadOfTip =
328+
Operations::Needle::PrunePointsAheadOfTip::get(itShaft->getTypeInfo());
329+
prunePointsAheadOfTip(m_couplingPts, itShaft->element());
330+
}
331+
}
332+
333+
InsertionAlgorithm::AlgorithmOutput InsertionAlgorithm::reprojectCouplingPoints()
334+
{
335+
if (m_couplingPts.empty() || !l_shaftGeom) return AlgorithmOutput();
336+
337+
AlgorithmOutput reprojectionOutput;
338+
339+
// Reprojection on shaft geometry sequence
340+
auto findClosestProxOnShaft = Operations::FindClosestProximity::Operation::get(l_shaftGeom);
341+
auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom);
342+
for (const auto& cp : m_couplingPts)
343+
{
344+
const BaseProximity::SPtr shaftProx =
345+
findClosestProxOnShaft(cp, l_shaftGeom.get(), projectOnShaft, getFilterFunc());
346+
if (!shaftProx) continue;
347+
shaftProx->normalize();
348+
reprojectionOutput.add(shaftProx, cp);
349+
}
350+
// This is a final-frontier check: If there are coupling points stored, but the
351+
// findClosestProxOnShaf operation yields no proximities on the shaft, it could be
352+
// because the needle has exited abruptly. Thus, we clear the coupling points.
353+
if (reprojectionOutput.size() == 0) m_couplingPts.clear();
354+
355+
return reprojectionOutput;
356+
}
357+
346358
} // namespace sofa::collisionalgorithm

‎src/CollisionAlgorithm/algorithm/InsertionAlgorithm.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -47,6 +47,8 @@ class SOFA_COLLISIONALGORITHM_API InsertionAlgorithm : public BaseAlgorithm
4747

4848
virtual AlgorithmOutput puncturePhase();
4949
virtual AlgorithmOutput shaftCollisionPhase();
50+
virtual void insertionPhase();
51+
virtual AlgorithmOutput reprojectCouplingPoints();
5052
};
5153

5254
} // namespace sofa::collisionalgorithm

0 commit comments

Comments
 (0)