diff --git a/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h b/src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h index 1a455834..fabc306b 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 @@ -225,24 +226,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); } } } 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..e03407a9 --- /dev/null +++ b/src/sofa/collisionAlgorithm/operations/ContainsPoint.h @@ -0,0 +1,74 @@ +#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 + { + msg_error("ContainsPointInElement") + << "The operation ContainsPointInElementOperation is not registered with for type = " + << sofa::helper::NameDecoder::decodeFullName(id); + } +}; + +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 + { + msg_error("ContainsPointInProximity") + << "The operation ContainsPointInProximityOperation is not registered with for type = " + << sofa::helper::NameDecoder::decodeFullName(id); + } +}; + +template +Result containsPoint(const type::Vec3& P, const typename std::shared_ptr& prox) +{ + if (!prox) + { + msg_warning("ContainsPointInProximity") << "Null proximity pointer in containsPoint" + << "Operation is disabled; returning false"; + return false; + } + + 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..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 { @@ -12,6 +13,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..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 { @@ -12,6 +13,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);