SubMesh::RecalculateNormals improvement

graphics/include/gz/common/SubMesh.hh

@@ -20,7 +20,6 @@
 #include <memory>
 #include <optional>
 #include <string>
-#include <vector>
 
 #include <gz/math/Vector3.hh>
 #include <gz/math/Vector2.hh>
@@ -158,7 +157,7 @@ namespace gz
       public: gz::math::Vector3d Vertex(const unsigned int _index) const;
 
       /// \brief Get the raw vertex pointer. This is unsafe, it is the
-      /// caller's responsability to ensure it's not indexed out of bounds.
+      /// caller's responsibility to ensure it's not indexed out of bounds.
       /// The valid range is [0; VertexCount())
       /// \return Raw vertices
       public: const gz::math::Vector3d* VertexPtr() const;
@@ -224,7 +223,7 @@ namespace gz
       public: int Index(const unsigned int _index) const;
 
       /// \brief Get the raw index pointer. This is unsafe, it is the
-      /// caller's responsability to ensure it's not indexed out of bounds.
+      /// caller's responsibility to ensure it's not indexed out of bounds.
       /// The valid range is [0; IndexCount())
       /// \return Raw indices
       public: const unsigned int* IndexPtr() const;
@@ -416,7 +415,7 @@ namespace gz
       GZ_UTILS_IMPL_PTR(dataPtr)
     };
 
-    /// \brief Vertex to node weighted assignement for skeleton animation
+    /// \brief Vertex to node weighted assignment for skeleton animation
     /// visualization
     class GZ_COMMON_GRAPHICS_VISIBLE NodeAssignment
     {

graphics/src/SubMesh.cc

@@ -18,8 +18,8 @@
 #include <algorithm>
 #include <limits>
 #include <map>
-#include <optional>
 #include <string>
+#include <vector>
 
 #include "gz/math/Helpers.hh"
 
@@ -573,10 +573,67 @@ void SubMesh::FillArrays(double **_vertArr, int **_indArr) const
   }
 }
 
+namespace {
+// Simple way to find neighbors by grouping all vertices
+// by X coordinate in (ordered) map. KD-tree maybe better
+// but not sure about construction overhead
+struct Neighbors
+{
+  Neighbors(const std::vector<unsigned int> &_indices,
+            const std::vector<gz::math::Vector3d> &_vertices)
+    : vertices(_vertices)
+  {
+    for (unsigned int i = 0; i < _indices.size(); ++i)
+    {
+      const auto index = _indices[i];
+      this->groups[_vertices[index].X()].push_back(index);
+    }
+  }
+
+  // When we have a concrete point to check, we are looking for
+  // a group inside a map with a same X.
+  // Then we check neighbors with the smaller X until
+  // it's in tolerance of the math::equal function.
+  // Starting from smallest X, which is in a tolerance range,
+  // testing all points in group for equality. In case of equality,
+  // call a Visitor with element index as an argument.
+  // Continue until a greater side of X tolerance range reached.
+  template<typename Visitor>
+  void Visit(const gz::math::Vector3d &_point, Visitor _v) const
+  {
+    auto it = this->groups.find(_point.X());
+    // find smaller acceptable value
+    while (it != this->groups.begin())
+    {
+      auto prev = it;
+      --prev;
+      if (!gz::math::equal(prev->first, _point.X()))
+        break;
+      it = prev;
+    }
+    while (it != this->groups.end()
+           && gz::math::equal(it->first, _point.X()))
+    {
+      for (const auto index : it->second)
+        if (this->vertices[index] == _point)
+          _v(index);
+      ++it;
+    }
+  }
+
+  // Indexes of vertices grouped by X coordinate
+  private: std::map<double, std::vector<unsigned int>> groups;
+  // Const reference to a vertices vector
+  private: const std::vector<gz::math::Vector3d> &vertices;
+};
+}  // namespace
+
 //////////////////////////////////////////////////
 void SubMesh::RecalculateNormals()
 {
-  if (this->dataPtr->normals.size() < 3u)
+  if (this->dataPtr->primitiveType != SubMesh::TRIANGLES
+      || this->dataPtr->indices.empty()
+      || this->dataPtr->indices.size() % 3u != 0)
     return;
 
   // Reset all the normals
@@ -586,6 +643,8 @@ void SubMesh::RecalculateNormals()
   if (this->dataPtr->normals.size() != this->dataPtr->vertices.size())
     this->dataPtr->normals.resize(this->dataPtr->vertices.size());
 
+  Neighbors neighbors(this->dataPtr->indices, this->dataPtr->vertices);
+
   // For each face, which is defined by three indices, calculate the normals
   for (unsigned int i = 0; i < this->dataPtr->indices.size(); i+= 3)
   {
@@ -597,14 +656,11 @@ void SubMesh::RecalculateNormals()
         this->dataPtr->vertices[this->dataPtr->indices[i+2]];
     gz::math::Vector3d n = gz::math::Vector3d::Normal(v1, v2, v3);
 
-    for (unsigned int j = 0; j < this->dataPtr->vertices.size(); ++j)
-    {
-      gz::math::Vector3d v = this->dataPtr->vertices[j];
-      if (v == v1 || v == v2 || v == v3)
+    for (const auto &point : {v1, v2, v3})
+      neighbors.Visit(point, [&](const unsigned int index)
       {
-        this->dataPtr->normals[j] += n;
-      }
-    }
+        this->dataPtr->normals[index] += n;
+      });
   }
 
   // Normalize the results

graphics/src/SubMesh_TEST.cc

@@ -585,3 +585,32 @@ TEST_F(SubMeshTest, Volume)
   boxSub.AddIndex(1);
   EXPECT_DOUBLE_EQ(0.0, boxSub.Volume());
 }
+
+/////////////////////////////////////////////////
+TEST_F(SubMeshTest, NormalsRecalculation)
+{
+  auto submesh = std::make_shared<common::SubMesh>();
+  submesh->SetPrimitiveType(common::SubMesh::TRIANGLES);
+
+  constexpr unsigned int triangles = 16384;
+  for (unsigned int i = 0; i < triangles; ++i) {
+    // sub to X less than _epsilon from even triangles
+    // expect that the 2nd vertex should be matched with
+    // the 1st of next triangle
+    const auto jitter = i % 2 ? 1e-7 : 0.0;
+    submesh->AddVertex(i-jitter, i, i);
+    submesh->AddVertex(i+1, i+1, i+1);
+    submesh->AddVertex(i, i, -static_cast<double>(i));
+
+    submesh->AddIndex(3*i);
+    submesh->AddIndex(3*i+1);
+    submesh->AddIndex(3*i+2);
+  }
+
+  ASSERT_EQ(submesh->IndexCount() % 3, 0u);
+  submesh->RecalculateNormals();
+  ASSERT_EQ(submesh->NormalCount(), submesh->VertexCount());
+  // Same triangle, but different normals
+  // because of neighbour vertex
+  ASSERT_NE(submesh->Normal(0), submesh->Normal(1));
+}