[SofaBaseMechanics] BarycentricMapping: spatial hashing, handle limit cases

SofaKernel/modules/SofaBaseMechanics/BarycentricMappers/BarycentricMapperTopologyContainer.h

@@ -102,20 +102,20 @@ class BarycentricMapperTopologyContainer : public TopologyBarycentricMapper<In,O
         }
 
         int xId,yId,zId; // cell indices
-        int elementId;
+        unsigned int elementId;
     };
 
     struct NearestParams
     {
         NearestParams()
         {
             distance = std::numeric_limits<double>::max();
-            elementIndex = -1;
+            elementId = UINT_MAX;
         }
 
         Vector3 baryCoords;
         double distance;
-        int elementIndex;
+        unsigned int elementId;
     };
 
     using Inherit1::m_fromTopology;
@@ -144,11 +144,20 @@ class BarycentricMapperTopologyContainer : public TopologyBarycentricMapper<In,O
     virtual void addPointInElement(const int elementIndex, const SReal* baryCoords)=0;
     virtual void computeDistance(double& d, const Vector3& v)=0;
 
+    /// Compute the distance between outPos and the element e. If this distance is smaller than the previously stored one,
+    /// update nearestParams.
+    /// \param e id of the element
+    /// \param outPos position of the point we want to compute the barycentric coordinates
+    /// \param inPos position of one point of the element
+    /// \param nearestParams output parameters (nearest element id, distance, and barycentric coordinates)
     void checkDistanceFromElement(unsigned int e,
                                   const Vector3& outPos,
                                   const Vector3& inPos,
                                   NearestParams& nearestParams);
 
+
+    /// Compute the datas needed to find the nearest element
+    /// \param in is the vector of points
     void computeBasesAndCenters( const typename In::VecCoord& in );
 
     // Spacial hashing following paper:

SofaKernel/modules/SofaBaseMechanics/BarycentricMappers/BarycentricMapperTopologyContainer.inl

@@ -171,13 +171,14 @@ void BarycentricMapperTopologyContainer<In,Out,MappingDataType,Element>::init (
             }
         }
 
-        if(nearestParams.elementIndex==-1) // No element in grid cell, perform exhaustive search
+        if(nearestParams.elementId==UINT_MAX) // No element in grid cell, perform exhaustive search
         {
             for ( unsigned int e = 0; e < elements.size(); e++ )
             {
                 Vector3 inPos = in[elements[e][0]];
                 checkDistanceFromElement(e, outPos, inPos, nearestParams);
             }
+            addPointInElement(nearestParams.elementId, nearestParams.baryCoords.ptr());
         }
         else if(abs(nearestParams.distance)>m_gridCellSize/2.) // Nearest element in grid cell may not be optimal, check neighbors
         {
@@ -199,9 +200,10 @@ void BarycentricMapperTopologyContainer<In,Out,MappingDataType,Element>::init (
                             }
                         }
                     }
+            addPointInElement(nearestParams.elementId, nearestParams.baryCoords.ptr());
         }
-
-        addPointInElement(nearestParams.elementIndex, nearestParams.baryCoords.ptr());
+        else
+            addPointInElement(nearestParams.elementId, nearestParams.baryCoords.ptr());
     }
 }
 
@@ -243,7 +245,7 @@ void BarycentricMapperTopologyContainer<In,Out,MappingDataType,Element>::checkDi
     {
         nearestParams.baryCoords = bary;
         nearestParams.distance = dist;
-        nearestParams.elementIndex = int(e);
+        nearestParams.elementId = e;
     }
 };