Pad using weighted vertex normals

include/geometric_shapes/shapes.h

@@ -336,6 +336,10 @@ class Mesh : public Shape
      Calls computeTriangleNormals() if needed. */
   void computeVertexNormals();
 
+  /** \brief Compute vertex normals by averaging from adjacent triangle normals, weighted using magnitude of
+   * angles formed by adjacent triangles at the vertex. Provides a more accurate result than computeVertexNormals() */
+  void computeWeightedVertexNormals();
+
   /** \brief Merge vertices that are very close to each other, up to a threshold*/
   void mergeVertices(double threshold);
 

src/mesh_operations.cpp

@@ -118,7 +118,7 @@ Mesh* createMeshFromVertices(const EigenSTL::vector_Vector3d& vertices, const st
 
   std::copy(triangles.begin(), triangles.end(), mesh->triangles);
   mesh->computeTriangleNormals();
-  mesh->computeVertexNormals();
+  mesh->computeWeightedVertexNormals();
 
   return mesh;
 }
@@ -194,7 +194,7 @@ Mesh* createMeshFromVertices(const EigenSTL::vector_Vector3d& source)
 
   std::copy(triangles.begin(), triangles.end(), mesh->triangles);
   mesh->computeTriangleNormals();
-  mesh->computeVertexNormals();
+  mesh->computeWeightedVertexNormals();
 
   return mesh;
 }
@@ -414,7 +414,7 @@ Mesh* createMeshFromShape(const Box& box)
                                       3, 4, 5, 5, 0, 3, 0, 5, 7, 7, 1, 0, 7, 5, 4, 4, 6, 7 };
   memcpy(result->triangles, tri, sizeof(unsigned int) * 36);
   result->computeTriangleNormals();
-  result->computeVertexNormals();
+  result->computeWeightedVertexNormals();
   return result;
 }
 

src/shape_operations.cpp

@@ -535,7 +535,7 @@ Shape* constructShapeFromText(std::istream& in)
           in >> m->triangles[i3] >> m->triangles[i3 + 1] >> m->triangles[i3 + 2];
         }
         m->computeTriangleNormals();
-        m->computeVertexNormals();
+        m->computeWeightedVertexNormals();
       }
       else
         CONSOLE_BRIDGE_logError("Unknown shape type: '%s'", type.c_str());

src/shapes.cpp

@@ -400,23 +400,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;
   }
 }
 
@@ -564,6 +556,72 @@ void Mesh::computeVertexNormals()
   }
 }
 
+void Mesh::computeWeightedVertexNormals()
+{
+  if (!triangle_normals)
+    computeTriangleNormals();
+  if (vertex_count && !vertex_normals)
+    vertex_normals = new double[vertex_count * 3];
+  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)
+  {
+    unsigned int tIdx3 = 3 * tIdx;
+    unsigned int tIdx3_1 = tIdx3 + 1;
+    unsigned int tIdx3_2 = tIdx3 + 2;
+
+    unsigned int v1 = triangles[tIdx3];
+    unsigned int v2 = triangles[tIdx3_1];
+    unsigned int v3 = triangles[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
+    mapped_normals.col(v1)[0] += triangle_normals[tIdx3] * ang1;
+    mapped_normals.col(v1)[1] += triangle_normals[tIdx3_1] * ang1;
+    mapped_normals.col(v1)[2] += triangle_normals[tIdx3_2] * ang1;
+
+    mapped_normals.col(v2)[0] += triangle_normals[tIdx3] * ang2;
+    mapped_normals.col(v2)[1] += triangle_normals[tIdx3_1] * ang2;
+    mapped_normals.col(v2)[2] += triangle_normals[tIdx3_2] * ang2;
+
+    mapped_normals.col(v3)[0] += triangle_normals[tIdx3] * ang3;
+    mapped_normals.col(v3)[1] += triangle_normals[tIdx3_1] * ang3;
+    mapped_normals.col(v3)[2] += triangle_normals[tIdx3_2] * ang3;
+  }
+
+  // Normalize each column of the matrix
+  for (int i = 0; i < mapped_normals.cols(); ++i)
+  {
+    auto mapped_normal = mapped_normals.col(i);
+    if (mapped_normal.squaredNorm() != 0.0)
+    {
+      mapped_normal.normalize();
+    }
+  }
+}
+
 void Mesh::mergeVertices(double threshold)
 {
   const double thresholdSQR = threshold * threshold;
@@ -623,7 +681,7 @@ void Mesh::mergeVertices(double threshold)
   if (triangle_normals)
     computeTriangleNormals();
   if (vertex_normals)
-    computeVertexNormals();
+    computeWeightedVertexNormals();
 }
 
 } /* namespace shapes */

test/test_shapes.cpp

@@ -276,10 +276,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);
@@ -314,7 +314,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);
@@ -423,10 +423,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);