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,70 @@ void Mesh::computeVertexNormals()
   }
 }
 
+void Mesh::computeWeightedVertexNormals()
+{
+  if (!triangle_normals)
+    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));
+
+  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::Vector3d p1{ vertices[3 * v1], vertices[3 * v1 + 1], vertices[3 * v1 + 2] };
+    Eigen::Vector3d p2{ vertices[3 * v2], vertices[3 * v2 + 1], vertices[3 * v2 + 2] };
+    Eigen::Vector3d p3{ vertices[3 * v3], vertices[3 * v3 + 1], vertices[3 * v3 + 2] };
+
+    {
+      // Use re-arranged dot product equation to calculate angle between two vectors
+      auto angleBetweenVectors = [](const Eigen::Vector3d& v1, const Eigen::Vector3d& v2) -> double {
+        return std::acos(v1.dot(v2) / (v1.norm() * v2.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
+      avg_normals[v1][0] += triangle_normals[tIdx3] * ang1;
+      avg_normals[v1][1] += triangle_normals[tIdx3_1] * ang1;
+      avg_normals[v1][2] += triangle_normals[tIdx3_2] * ang1;
+
+      avg_normals[v2][0] += triangle_normals[tIdx3] * ang2;
+      avg_normals[v2][1] += triangle_normals[tIdx3_1] * ang2;
+      avg_normals[v2][2] += triangle_normals[tIdx3_2] * ang2;
+
+      avg_normals[v3][0] += triangle_normals[tIdx3] * ang3;
+      avg_normals[v3][1] += triangle_normals[tIdx3_1] * ang3;
+      avg_normals[v3][2] += triangle_normals[tIdx3_2] * ang3;
+    }
+  }
+
+  for (std::size_t i = 0; i < avg_normals.size(); ++i)
+  {
+    auto& avg_normal = avg_normals[i];
+    if (avg_normal.squaredNorm() != 0.0)
+    {
+      avg_normal.normalize();
+    }
+
+    unsigned int i3 = i * 3;
+    vertex_normals[i3] = avg_normal[0];
+    vertex_normals[i3 + 1] = avg_normal[1];
+    vertex_normals[i3 + 2] = avg_normal[2];
+  }
+}
+
 void Mesh::mergeVertices(double threshold)
 {
   const double thresholdSQR = threshold * threshold;
@@ -623,7 +679,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);