Improve padding of meshes using weighted vertex normals (moveit#238)

include/geometric_shapes/shapes.h

@@ -321,7 +321,8 @@ class Mesh : public Shape
   /** \brief Compute the normals of each triangle from its vertices via cross product. */
   void computeTriangleNormals();
 
-  /** \brief Compute vertex normals by averaging from adjacent triangle normals.
+  /** \brief Compute vertex normals by averaging from adjacent triangle normals, weighted using magnitude of
+   * angles formed by adjacent triangles at the vertex.
 
      Calls computeTriangleNormals() if needed. */
   void computeVertexNormals();

src/shapes.cpp

@@ -394,23 +394,15 @@ void Mesh::scaleAndPadd(double scaleX, double scaleY, double scaleZ, double padd
     double dy = vertices[i3 + 1] - sy;
     double dz = vertices[i3 + 2] - sz;
 
-    // length of vector
-    double norm = sqrt(dx * dx + dy * dy + dz * dz);
-    if (norm > 1e-6)
-    {
-      vertices[i3] = sx + dx * (scaleX + paddX / norm);
-      vertices[i3 + 1] = sy + dy * (scaleY + paddY / norm);
-      vertices[i3 + 2] = sz + dz * (scaleZ + paddZ / norm);
-    }
-    else
-    {
-      double ndx = ((dx > 0) ? dx + paddX : dx - paddX);
-      double ndy = ((dy > 0) ? dy + paddY : dy - paddY);
-      double ndz = ((dz > 0) ? dz + paddZ : dz - paddZ);
-      vertices[i3] = sx + ndx;
-      vertices[i3 + 1] = sy + ndy;
-      vertices[i3 + 2] = sz + ndz;
-    }
+    // Scaled coordinate
+    double scaledX = sx + dx * scaleX;
+    double scaledY = sy + dy * scaleY;
+    double scaledZ = sz + dz * scaleZ;
+
+    // Padding in each direction
+    vertices[i3] = scaledX + vertex_normals[i3] * paddX;
+    vertices[i3 + 1] = scaledY + vertex_normals[i3 + 1] * paddY;
+    vertices[i3 + 2] = scaledZ + vertex_normals[i3 + 2] * paddZ;
   }
 }
 
@@ -523,7 +515,8 @@ void Mesh::computeVertexNormals()
     computeTriangleNormals();
   if (vertex_count && !vertex_normals)
     vertex_normals = new double[vertex_count * 3];
-  EigenSTL::vector_Vector3d avg_normals(vertex_count, Eigen::Vector3d(0, 0, 0));
+  Eigen::Map<Eigen::Matrix<double, 3, Eigen::Dynamic>> mapped_normals(vertex_normals, 3, vertex_count);
+  mapped_normals.setZero();
 
   for (unsigned int tIdx = 0; tIdx < triangle_count; ++tIdx)
   {
@@ -535,26 +528,43 @@ void Mesh::computeVertexNormals()
     unsigned int v2 = triangles[tIdx3_1];
     unsigned int v3 = triangles[tIdx3_2];
 
-    avg_normals[v1][0] += triangle_normals[tIdx3];
-    avg_normals[v1][1] += triangle_normals[tIdx3_1];
-    avg_normals[v1][2] += triangle_normals[tIdx3_2];
-
-    avg_normals[v2][0] += triangle_normals[tIdx3];
-    avg_normals[v2][1] += triangle_normals[tIdx3_1];
-    avg_normals[v2][2] += triangle_normals[tIdx3_2];
-
-    avg_normals[v3][0] += triangle_normals[tIdx3];
-    avg_normals[v3][1] += triangle_normals[tIdx3_1];
-    avg_normals[v3][2] += triangle_normals[tIdx3_2];
+    // Get angles for each vertex at this triangle
+    Eigen::Map<Eigen::Vector3d> p1{ vertices + 3 * v1, 3 };
+    Eigen::Map<Eigen::Vector3d> p2{ vertices + 3 * v2, 3 };
+    Eigen::Map<Eigen::Vector3d> p3{ vertices + 3 * v3, 3 };
+
+    // Use re-arranged dot product equation to calculate angle between two vectors
+    auto angleBetweenVectors = [](const Eigen::Vector3d& vec1, const Eigen::Vector3d& vec2) -> double {
+      double vec1_norm = vec1.norm();
+      double vec2_norm = vec2.norm();
+
+      // Handle the case where either vector has zero length, to prevent division-by-zero
+      if (vec1_norm == 0.0 || vec2_norm == 0.0)
+        return 0.0;
+
+      return std::acos(vec1.dot(vec2) / (vec1_norm * vec2_norm));
+    };
+
+    // Use law of cosines to compute angles
+    auto ang1 = angleBetweenVectors(p2 - p1, p3 - p1);
+    auto ang2 = angleBetweenVectors(p1 - p2, p3 - p2);
+    auto ang3 = angleBetweenVectors(p1 - p3, p2 - p3);
+
+    // Weight normal with angle
+    Eigen::Map<Eigen::Vector3d> triangle_normal{ triangle_normals + tIdx3, 3 };
+    mapped_normals.col(v1) += triangle_normal * ang1;
+    mapped_normals.col(v2) += triangle_normal * ang2;
+    mapped_normals.col(v3) += triangle_normal * ang3;
   }
-  for (std::size_t i = 0; i < avg_normals.size(); ++i)
+
+  // Normalize each column of the matrix
+  for (int i = 0; i < mapped_normals.cols(); ++i)
   {
-    if (avg_normals[i].squaredNorm() > 0.0)
-      avg_normals[i].normalize();
-    unsigned int i3 = i * 3;
-    vertex_normals[i3] = avg_normals[i][0];
-    vertex_normals[i3 + 1] = avg_normals[i][1];
-    vertex_normals[i3 + 2] = avg_normals[i][2];
+    auto mapped_normal = mapped_normals.col(i);
+    if (mapped_normal.squaredNorm() != 0.0)
+    {
+      mapped_normal.normalize();
+    }
   }
 }
 

test/test_shapes.cpp

@@ -270,10 +270,10 @@ TEST(Mesh, ScaleAndPadd)
   EXPECT_DOUBLE_EQ(mesh2->vertices[22], -2.0);
   EXPECT_DOUBLE_EQ(mesh2->vertices[23], 2.0);
 
-  // padding actually means extending each vertices' direction vector by the padding value,
-  // not extending it along each axis by the same amount
+  // for a right-angled corner, the vertex normal vector points away equally from the three sides, and hence
+  // padding is applied equally in x, y and z, such that the total distance the vertex moves is equal to 1.0.
   mesh2->padd(1.0);
-  const double pos = 2.0 * (1 + 1.0 / sqrt(12));
+  const double pos = 2.0 + 1.0 / sqrt(3);
 
   EXPECT_DOUBLE_EQ(mesh2->vertices[0], pos);
   EXPECT_DOUBLE_EQ(mesh2->vertices[1], pos);
@@ -308,7 +308,7 @@ TEST(Mesh, ScaleAndPadd)
   EXPECT_DOUBLE_EQ(mesh2->vertices[23], pos);
 
   mesh2->scaleAndPadd(2.0, 1.0);
-  const double pos2 = pos * (2.0 + 1.0 / sqrt(3 * pos * pos));
+  const double pos2 = pos * 2.0 + 1.0 / sqrt(3);
 
   EXPECT_DOUBLE_EQ(mesh2->vertices[0], pos2);
   EXPECT_DOUBLE_EQ(mesh2->vertices[1], pos2);
@@ -417,10 +417,9 @@ TEST(Mesh, ScaleAndPadd)
   EXPECT_DOUBLE_EQ(mesh2->vertices[23], pos4z);
 
   mesh2->padd(1.0, 2.0, 3.0);
-  const double norm5 = sqrt(pos4x * pos4x + pos4y * pos4y + pos4z * pos4z);
-  const double pos5x = pos4x * (1.0 + 1.0 / norm5);
-  const double pos5y = pos4y * (1.0 + 2.0 / norm5);
-  const double pos5z = pos4z * (1.0 + 3.0 / norm5);
+  const double pos5x = pos4x + (1.0 / sqrt(3));
+  const double pos5y = pos4y + (2.0 / sqrt(3));
+  const double pos5z = pos4z + (3.0 / sqrt(3));
 
   EXPECT_DOUBLE_EQ(mesh2->vertices[0], pos5x);
   EXPECT_DOUBLE_EQ(mesh2->vertices[1], pos5y);