diff --git a/graphics/include/gz/common/SubMesh.hh b/graphics/include/gz/common/SubMesh.hh index 938a93d2..3915997f 100644 --- a/graphics/include/gz/common/SubMesh.hh +++ b/graphics/include/gz/common/SubMesh.hh @@ -20,7 +20,6 @@ #include #include #include -#include #include #include @@ -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 { diff --git a/graphics/src/SubMesh.cc b/graphics/src/SubMesh.cc index 53726dc0..23fc8bad 100644 --- a/graphics/src/SubMesh.cc +++ b/graphics/src/SubMesh.cc @@ -18,8 +18,8 @@ #include #include #include -#include #include +#include #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 &_indices, + const std::vector &_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 + 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> groups; + // Const reference to a vertices vector + private: const std::vector &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 diff --git a/graphics/src/SubMesh_TEST.cc b/graphics/src/SubMesh_TEST.cc index 1d332f91..eb07afed 100644 --- a/graphics/src/SubMesh_TEST.cc +++ b/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(); + 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(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)); +}