code cleanup; fix bug invalidating a face

seerep_hdf5/seerep_hdf5_core/src/hdf5_core_image.cpp

@@ -218,11 +218,26 @@ Hdf5CoreImage::computeFrustumMesh(std::array<seerep_core_msgs::Point, 5>& points
   auto br = mesh.add_vertex(
       CGPoint_3{ points[4].get<0>(), points[4].get<1>(), points[4].get<2>() });
 
-  mesh.add_face(o, tl, tr);
-  mesh.add_face(o, tr, br);
-  mesh.add_face(o, br, bl);
-  mesh.add_face(o, bl, tl);
-  mesh.add_face(tl, tr, br, bl);
+  std::vector<CGSurfaceMesh::Face_index> descriptors;
+
+  descriptors.push_back(mesh.add_face(o, tl, tr));
+  descriptors.push_back(mesh.add_face(o, tr, br));
+  descriptors.push_back(mesh.add_face(o, br, bl));
+  descriptors.push_back(mesh.add_face(o, bl, tl));
+  // for whatever reason the face needs to be constructed in this way,
+  // when the add_face returns a null_face() ALWAYS try reversing the vertex
+  // order
+  descriptors.push_back(mesh.add_face(bl, br, tr, tl));
+
+  // check if any of the faces was not constructed properly
+  if (std::find_if(descriptors.begin(), descriptors.end(), [](auto elem) {
+        return elem == CGSurfaceMesh::null_face();
+      }) != descriptors.end())
+  {
+    throw std::invalid_argument("Could not create the faces for the "
+                                "SurfaceMesh from the camera frustum points!");
+  }
+
   return mesh;
 }
 

seerep_hdf5/seerep_hdf5_core/src/hdf5_core_point_cloud.cpp

@@ -117,48 +117,30 @@ Hdf5CorePointCloud::createMeshFromAABB(const std::vector<float>& bb_coords)
     verts.push_back(mesh.add_vertex(p));
   }
 
+  std::vector<CGSurfaceMesh::Face_index> descriptors;
+
   // bottom plane
-  auto desc = mesh.add_face(verts[0], verts[4], verts[6], verts[2]);
-  if (desc == CGSurfaceMesh::null_face())
-  {
-    throw std::invalid_argument(
-        "Could not add bottom face from AABB to SurfaceMesh correctly!");
-  }
+  descriptors.push_back(mesh.add_face(verts[0], verts[4], verts[6], verts[2]));
   // front plane
-  mesh.add_face(verts[0], verts[1], verts[3], verts[2]);
-  if (desc == CGSurfaceMesh::null_face())
-  {
-    throw std::invalid_argument(
-        "Could not add front face from AABB to SurfaceMesh correctly!");
-  }
+  descriptors.push_back(mesh.add_face(verts[0], verts[1], verts[3], verts[2]));
   // left plane
-  mesh.add_face(verts[4], verts[5], verts[1], verts[0]);
-  if (desc == CGSurfaceMesh::null_face())
-  {
-    throw std::invalid_argument(
-        "Could not add left face from AABB to SurfaceMesh correctly!");
-  }
+  descriptors.push_back(mesh.add_face(verts[4], verts[5], verts[1], verts[0]));
   // back plane
-  mesh.add_face(verts[4], verts[5], verts[7], verts[6]);
-  if (desc == CGSurfaceMesh::null_face())
-  {
-    throw std::invalid_argument(
-        "Could not add back face from AABB to SurfaceMesh correctly!");
-  }
+  descriptors.push_back(mesh.add_face(verts[4], verts[5], verts[7], verts[6]));
   // right plane
-  mesh.add_face(verts[2], verts[3], verts[7], verts[6]);
-  if (desc == CGSurfaceMesh::null_face())
-  {
-    throw std::invalid_argument(
-        "Could not add right face from AABB to SurfaceMesh correctly!");
-  }
+  descriptors.push_back(mesh.add_face(verts[2], verts[3], verts[7], verts[6]));
   // top plane
-  mesh.add_face(verts[1], verts[5], verts[7], verts[3]);
-  if (desc == CGSurfaceMesh::null_face())
+  descriptors.push_back(mesh.add_face(verts[1], verts[5], verts[7], verts[3]));
+
+  // check if any of the faces was not constructed properly
+  if (std::find_if(descriptors.begin(), descriptors.end(), [](auto elem) {
+        return elem == CGSurfaceMesh::null_face();
+      }) != descriptors.end())
   {
-    throw std::invalid_argument(
-        "Could not add top face from AABB to SurfaceMesh correctly!");
+    throw std::invalid_argument("Could not create the faces for the "
+                                "SurfaceMesh from the pointcloud AABB!");
   }
+
   return mesh;
 }
 

seerep_srv/seerep_core/src/core_dataset.cpp

@@ -1078,28 +1078,28 @@ CoreDataset::toSurfaceMesh(const seerep_core_msgs::Polygon2D& seerep_polygon)
   upper_surface.push_back(topFirst);
   lower_surface.push_back(bottomFirst);
 
-  face_desc =
-      surface_mesh.add_face(topFirst, topInitial, bottomInitial, bottomFirst);
-  if (face_desc == CGSurfaceMesh::null_face())
-  {
-    throw std::invalid_argument("Could not add last side face from Polygon2d "
-                                "to SurfaceMesh correctly!");
-  }
-  face_desc = surface_mesh.add_face(lower_surface);
-  if (face_desc == CGSurfaceMesh::null_face())
-  {
-    throw std::invalid_argument("Could not add lower surface face from "
-                                "Polygon2d to SurfaceMesh correctly!");
-  }
+  std::vector<CGSurfaceMesh::Face_index> descriptors;
+
+  descriptors.push_back(
+      surface_mesh.add_face(topFirst, topInitial, bottomInitial, bottomFirst));
+  descriptors.push_back(surface_mesh.add_face(lower_surface));
 
+  // for whatever reason the vertices need to be reversed for the latest face to
+  // construct the mesh correctly.
+  // If the construction of the latest face does not work ALWAYS try
+  // reversing the vertex order
   std::reverse(upper_surface.begin(), upper_surface.end());
+  descriptors.push_back(surface_mesh.add_face(upper_surface));
 
-  face_desc = surface_mesh.add_face(upper_surface);
-  if (face_desc == CGSurfaceMesh::null_face())
+  // check if any of the faces was not constructed properly
+  if (std::find_if(descriptors.begin(), descriptors.end(), [](auto elem) {
+        return elem == CGSurfaceMesh::null_face();
+      }) != descriptors.end())
   {
-    throw std::invalid_argument("Could not add upper surface face from "
-                                "Polygon2d to SurfaceMesh correctly!");
+    throw std::invalid_argument("Could not upper/lower or latest side face from"
+                                " Polygon2d to SurfaceMesh correctly!");
   }
+
   return surface_mesh;
 }