Merge pull request #621 from gazebosim/merge_5_main_20240801

Merge 5 -> main

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;
@@ -410,7 +409,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"
 
@@ -566,10 +566,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
@@ -579,6 +636,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)
   {
@@ -590,14 +649,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

@@ -569,3 +569,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));
+}

graphics/src/VHACD/VHACD.h

@@ -113,6 +113,12 @@
 #include <stdint.h>
 #include <functional>
 
+// Local modification done by @iche033
+// The m_voxelHullCount variable type is changed from
+// uint32_t to std::atomic<uint32_t>
+// to fix data race issue picked up by TSAN
+#include <atomic>
+
 #include <vector>
 #include <array>
 #include <cmath>
@@ -5965,12 +5971,12 @@ class VoxelHull
     std::unordered_map<uint32_t, uint32_t>      m_voxelIndexMap; // Maps from a voxel coordinate space into a vertex index space
     std::vector<VHACD::Vertex>                  m_vertices;
     std::vector<VHACD::Triangle>                m_indices;
-    static uint32_t                             m_voxelHullCount;
+    static std::atomic<uint32_t>                m_voxelHullCount;
     IVHACD::Parameters                          m_params;
     VHACDCallbacks*                             m_callbacks{ nullptr };
 };
 
-uint32_t VoxelHull::m_voxelHullCount = 0;
+std::atomic<uint32_t> VoxelHull::m_voxelHullCount = 0;
 
 VoxelHull::VoxelHull(const VoxelHull& parent,
                      SplitAxis axis,