[algorithm] Check tetProx after downcasting to TetrahedronProximity

Author: th-skam

src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h

@@ -155,7 +155,8 @@ class InsertionAlgorithm : public BaseAlgorithm
                         const auto& lambda =
                             m_constraintSolver->getLambda()[mstate.get()].read()->getValue();
                         SReal norm{0_sreal};
-                        for (const auto& l : lambda) {
+                        for (const auto& l : lambda)
+                        {
                             norm += l.norm();
                         }
                         if (norm > punctureForceThreshold)
@@ -179,9 +180,9 @@ class InsertionAlgorithm : public BaseAlgorithm
             // 1.3 Collision with the shaft geometry
             if (collisionOutput.size())
             {
-            auto createShaftProximity =
-                Operations::CreateCenterProximity::Operation::get(l_shaftGeom->getTypeInfo());
-            auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom);
+                auto createShaftProximity =
+                    Operations::CreateCenterProximity::Operation::get(l_shaftGeom->getTypeInfo());
+                auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom);
                 for (auto itShaft = l_shaftGeom->begin(); itShaft != l_shaftGeom->end(); itShaft++)
                 {
                     BaseProximity::SPtr shaftProx = createShaftProximity(itShaft->element());
@@ -191,11 +192,12 @@ class InsertionAlgorithm : public BaseAlgorithm
                     if (surfProx)
                     {
                         surfProx->normalize();
-    
+
                         // 1.2 If not, create a proximity pair for the tip-surface collision
                         if (d_projective.getValue())
                         {
-                            shaftProx = projectOnShaft(surfProx->getPosition(), itShaft->element()).prox;
+                            shaftProx =
+                                projectOnShaft(surfProx->getPosition(), itShaft->element()).prox;
                             if (!shaftProx) continue;
                             shaftProx->normalize();
                         }
@@ -225,21 +227,24 @@ class InsertionAlgorithm : public BaseAlgorithm
                 {
                     TetrahedronProximity::SPtr tetProx =
                         dynamic_pointer_cast<TetrahedronProximity>(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)
+                    if (tetProx)
                     {
-                        volProx->normalize();
-                        m_couplingPts.push_back(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);
+                        }
                     }
                 }
             }
-            else // Don't bother with removing the point that was just added
+            else  // Don't bother with removing the point that was just added
             {
-                // 2.2. Check whether coupling point should be removed 
+                // 2.2. Check whether coupling point should be removed
                 ElementIterator::SPtr itShaft = l_shaftGeom->begin(l_shaftGeom->getSize() - 2);
                 auto createShaftProximity =
                     Operations::CreateCenterProximity::Operation::get(itShaft->getTypeInfo());
@@ -248,7 +253,8 @@ class InsertionAlgorithm : public BaseAlgorithm
                 const type::Vec3 normal = (edgeProx->element()->getP1()->getPosition() -
                                            edgeProx->element()->getP0()->getPosition())
                                               .normalized();
-                if (dot(tip2Pt, normal) > 0_sreal) {
+                if (dot(tip2Pt, normal) > 0_sreal)
+                {
                     m_couplingPts.pop_back();
                 }
             }