From 3fae9afe6728b025a69c09333f332b2f12428f0c Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Mon, 8 Sep 2025 15:00:07 +0200 Subject: [PATCH 1/5] [operations] Created a ContainsPoint operation * Checks whether a point is inside an element * Works with an element or a proximity type as input --- .../operations/ContainsPoint.cpp | 25 +++++++ .../operations/ContainsPoint.h | 66 +++++++++++++++++++ .../toolbox/TetrahedronToolBox.cpp | 6 ++ .../toolbox/TetrahedronToolBox.h | 2 + .../toolbox/TriangleToolBox.cpp | 6 ++ .../toolbox/TriangleToolBox.h | 2 + 6 files changed, 107 insertions(+) create mode 100644 src/sofa/collisionAlgorithm/operations/ContainsPoint.cpp create mode 100644 src/sofa/collisionAlgorithm/operations/ContainsPoint.h diff --git a/src/sofa/collisionAlgorithm/operations/ContainsPoint.cpp b/src/sofa/collisionAlgorithm/operations/ContainsPoint.cpp new file mode 100644 index 00000000..b6147edb --- /dev/null +++ b/src/sofa/collisionAlgorithm/operations/ContainsPoint.cpp @@ -0,0 +1,25 @@ +#include +#include +#include + +namespace sofa::collisionAlgorithm::Operations::ContainsPointInElement +{ + +int register_ContainsPoint_Triangle = + Operation::register_func(&toolbox::TriangleToolBox::containsPoint); + +int register_ContainsPoint_Tetrahedron = + Operation::register_func(&toolbox::TetrahedronToolBox::containsPoint); + +} // namespace sofa::collisionAlgorithm::Operations::ContainsPointInElement + +namespace sofa::collisionAlgorithm::Operations::ContainsPointInProximity +{ + +int register_ContainsPointInProximity_Triangle = + Operation::register_func(&containsPoint); + +int register_ContainsPointInProximity_Tetrahedron = + Operation::register_func(&containsPoint); + +} // namespace sofa::collisionAlgorithm::Operations::ContainsPointInProximity diff --git a/src/sofa/collisionAlgorithm/operations/ContainsPoint.h b/src/sofa/collisionAlgorithm/operations/ContainsPoint.h new file mode 100644 index 00000000..4cd527d8 --- /dev/null +++ b/src/sofa/collisionAlgorithm/operations/ContainsPoint.h @@ -0,0 +1,66 @@ +#pragma once + +#include +#include + +namespace sofa::collisionAlgorithm::Operations::ContainsPointInElement +{ + +typedef bool Result; + +class Operation : public GenericOperation +{ + public: + Result defaultFunc(const type::Vec3&, const BaseElement::SPtr&) const override { return false; } + + void notFound(const std::type_info& id) const override + { + std::cerr << "ERROR the operation ContainsPointOperation is not registered with for type = " + << sofa::helper::NameDecoder::decodeFullName(id) << std::endl; + } +}; + +typedef Operation::FUNC FUNC; + +} // namespace sofa::collisionAlgorithm::Operations::ContainsPointInElement + +namespace sofa::collisionAlgorithm::Operations::ContainsPointInProximity +{ + +typedef bool Result; + +class Operation + : public GenericOperation +{ + public: + Result defaultFunc(const type::Vec3&, const BaseProximity::SPtr&) const override + { + return false; + } + + void notFound(const std::type_info& id) const override + { + std::cerr << "ERROR the operation ContainsPointProximityOperation is not registered with " + "for type = " + << sofa::helper::NameDecoder::decodeFullName(id) << std::endl; + } +}; + +template +Result containsPoint(const type::Vec3& P, const typename std::shared_ptr& prox) +{ + auto elem = prox->element(); + auto containsPointInElem = + sofa::collisionAlgorithm::Operations::ContainsPointInElement::Operation::get(elem); + return containsPointInElem(P, elem); +} + +typedef Operation::FUNC FUNC; + +} // namespace sofa::collisionAlgorithm::Operations::ContainsPointInProximity diff --git a/src/sofa/collisionAlgorithm/toolbox/TetrahedronToolBox.cpp b/src/sofa/collisionAlgorithm/toolbox/TetrahedronToolBox.cpp index ca1cb2e6..8cb12f9a 100644 --- a/src/sofa/collisionAlgorithm/toolbox/TetrahedronToolBox.cpp +++ b/src/sofa/collisionAlgorithm/toolbox/TetrahedronToolBox.cpp @@ -10,6 +10,12 @@ Operations::CreateCenterProximity::Result TetrahedronToolBox::createCenterProxim return TetrahedronProximity::create(tetra, 1.0/4.0,1.0/4.0,1.0/4.0,1.0/4.0); } +Operations::ContainsPointInElement::Result TetrahedronToolBox::containsPoint(const type::Vec3 & P, const TetrahedronElement::SPtr & tetra) { + TetrahedronProximity::SPtr prox = TetrahedronProximity::create(tetra, 1.0/4.0,1.0/4.0,1.0/4.0,1.0/4.0); + double f0(prox->f0()), f1(prox->f1()), f2(prox->f2()), f3(prox->f3()); + return isInTetra(P,tetra->getTetrahedronInfo(),f0,f1,f2,f3); +} + Operations::Project::Result TetrahedronToolBox::project(const type::Vec3 & P, const TetrahedronElement::SPtr & tetra) { double fact[4]; projectOnTetra(P, tetra->getTetrahedronInfo(),fact[0],fact[1],fact[2],fact[3]); diff --git a/src/sofa/collisionAlgorithm/toolbox/TetrahedronToolBox.h b/src/sofa/collisionAlgorithm/toolbox/TetrahedronToolBox.h index cceea96d..4f87ef13 100644 --- a/src/sofa/collisionAlgorithm/toolbox/TetrahedronToolBox.h +++ b/src/sofa/collisionAlgorithm/toolbox/TetrahedronToolBox.h @@ -12,6 +12,8 @@ class TetrahedronToolBox { static Operations::CreateCenterProximity::Result createCenterProximity(const TetrahedronElement::SPtr & tetra); + static Operations::ContainsPointInElement::Result containsPoint(const type::Vec3 & P, const TetrahedronElement::SPtr & tetra); + static Operations::Project::Result project(const type::Vec3 & P, const TetrahedronElement::SPtr & tetra); static void projectOnTetra(const type::Vec3d projectP, const TetrahedronElement::TetraInfo & teinfo, double & fact_u, double & fact_v, double & fact_w,double & fact_x); diff --git a/src/sofa/collisionAlgorithm/toolbox/TriangleToolBox.cpp b/src/sofa/collisionAlgorithm/toolbox/TriangleToolBox.cpp index da22a492..8af3bc7e 100644 --- a/src/sofa/collisionAlgorithm/toolbox/TriangleToolBox.cpp +++ b/src/sofa/collisionAlgorithm/toolbox/TriangleToolBox.cpp @@ -8,6 +8,12 @@ Operations::CreateCenterProximity::Result TriangleToolBox::createCenterProximity return TriangleProximity::create(tri, 1.0/3.0,1.0/3.0,1.0/3.0); } +Operations::ContainsPointInElement::Result TriangleToolBox::containsPoint(const type::Vec3 & P, const TriangleElement::SPtr & tri) { + TriangleProximity::SPtr prox = TriangleProximity::create(tri, 1.0 / 3.0, 1.0 / 3.0, 1.0 / 3.0); + double f0(prox->f0()), f1(prox->f1()), f2(prox->f2()); + return isInTriangle(P,tri->getTriangleInfo(),f0,f1,f1); +} + //Barycentric coordinates are computed according to //http://gamedev.stackexchange.com/questions/23743/whats-the-most-efficient-way-to-find-barycentric-coordinates Operations::Project::Result TriangleToolBox::project(const type::Vec3 & P, const TriangleElement::SPtr & tri) { diff --git a/src/sofa/collisionAlgorithm/toolbox/TriangleToolBox.h b/src/sofa/collisionAlgorithm/toolbox/TriangleToolBox.h index ccf162fd..05dde513 100644 --- a/src/sofa/collisionAlgorithm/toolbox/TriangleToolBox.h +++ b/src/sofa/collisionAlgorithm/toolbox/TriangleToolBox.h @@ -12,6 +12,8 @@ class TriangleToolBox { static Operations::CreateCenterProximity::Result createCenterProximity(const TriangleElement::SPtr & tri); + static Operations::ContainsPointInElement::Result containsPoint(const type::Vec3 & P, const TriangleElement::SPtr & tri); + static Operations::Project::Result project(const type::Vec3 & P, const TriangleElement::SPtr & tri); static void computeTriangleBaryCoords(const type::Vec3d & proj_P, const TriangleElement::TriangleInfo & tinfo, double & fact_u, double & fact_v, double & fact_w); From 0a932e089b2917b650c5ca2e9dc4f951d3a3d8b8 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Mon, 8 Sep 2025 15:10:48 +0200 Subject: [PATCH 2/5] [toolbox] Added a containsPoint function for point-in-tetra operation --- src/sofa/collisionAlgorithm/toolbox/TetrahedronToolBox.h | 1 + src/sofa/collisionAlgorithm/toolbox/TriangleToolBox.h | 1 + 2 files changed, 2 insertions(+) diff --git a/src/sofa/collisionAlgorithm/toolbox/TetrahedronToolBox.h b/src/sofa/collisionAlgorithm/toolbox/TetrahedronToolBox.h index 4f87ef13..75d6e701 100644 --- a/src/sofa/collisionAlgorithm/toolbox/TetrahedronToolBox.h +++ b/src/sofa/collisionAlgorithm/toolbox/TetrahedronToolBox.h @@ -4,6 +4,7 @@ #include #include #include +#include namespace sofa::collisionAlgorithm::toolbox { diff --git a/src/sofa/collisionAlgorithm/toolbox/TriangleToolBox.h b/src/sofa/collisionAlgorithm/toolbox/TriangleToolBox.h index 05dde513..f5bc8598 100644 --- a/src/sofa/collisionAlgorithm/toolbox/TriangleToolBox.h +++ b/src/sofa/collisionAlgorithm/toolbox/TriangleToolBox.h @@ -4,6 +4,7 @@ #include #include #include +#include namespace sofa::collisionAlgorithm::toolbox { From 0da5a7adc4fd959b7ef4a251609efdb4f00a61c4 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Mon, 8 Sep 2025 23:06:19 +0200 Subject: [PATCH 3/5] [algorithm] Using containsPoint operations in the insertion algorithm class - no dynamic casts to elements --- .../algorithm/InsertionAlgorithm.h | 20 +++++++------------ 1 file changed, 7 insertions(+), 13 deletions(-) diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 2ab650fb..8ea0da9d 100644 --- a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h +++ b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h @@ -6,6 +6,7 @@ #include #include #include +#include #include #include #include @@ -224,24 +225,17 @@ class InsertionAlgorithm : public BaseAlgorithm auto projectOnVol = Operations::Project::Operation::get(l_volGeom); const BaseProximity::SPtr volProx = findClosestProxOnVol(tipProx, l_volGeom.get(), projectOnVol, getFilterFunc()); + // Proximity can be detected before the tip enters the tetra (e.g. near a boundary face) // Only accept proximities if the tip is inside the tetra during insertion if (volProx) { - TetrahedronProximity::SPtr tetProx = - dynamic_pointer_cast(volProx); - if (tetProx) + auto containsPointInVol = Operations::ContainsPointInProximity::Operation::get( + l_volGeom->getTypeInfo()); + if(containsPointInVol(tipProx->getPosition(), volProx)) { - double f0(tetProx->f0()), f1(tetProx->f1()), f2(tetProx->f2()), - f3(tetProx->f3()); - bool isInTetra = toolbox::TetrahedronToolBox::isInTetra( - tipProx->getPosition(), tetProx->element()->getTetrahedronInfo(), f0, - f1, f2, f3); - if (isInTetra) - { - volProx->normalize(); - m_couplingPts.push_back(volProx); - } + volProx->normalize(); + m_couplingPts.push_back(volProx); } } } From 7f5518eafe8279c56a2c34233ceb0e97e147ee30 Mon Sep 17 00:00:00 2001 From: Themis Skamagkis Date: Thu, 11 Sep 2025 10:04:39 +0200 Subject: [PATCH 4/5] [operations] Use SOFA-like error messaging --- .../collisionAlgorithm/operations/ContainsPoint.h | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/sofa/collisionAlgorithm/operations/ContainsPoint.h b/src/sofa/collisionAlgorithm/operations/ContainsPoint.h index 4cd527d8..9450b22d 100644 --- a/src/sofa/collisionAlgorithm/operations/ContainsPoint.h +++ b/src/sofa/collisionAlgorithm/operations/ContainsPoint.h @@ -18,8 +18,9 @@ class Operation : public GenericOperationelement(); auto containsPointInElem = sofa::collisionAlgorithm::Operations::ContainsPointInElement::Operation::get(elem);