From 5731114871d15d3b3ff23657f3ef8641c2f692b6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?I=C3=B1igo=20Moreno?= <inigo.moreno@tecnalia.com>
Date: Wed, 25 Jan 2023 18:36:43 +0100
Subject: [PATCH 1/2] Allow bypassing averaging of normals

---
 .../raster/plane_slicer_raster_planner.cpp    | 64 +++++++++++--------
 1 file changed, 39 insertions(+), 25 deletions(-)

diff --git a/noether_tpp/src/tool_path_planners/raster/plane_slicer_raster_planner.cpp b/noether_tpp/src/tool_path_planners/raster/plane_slicer_raster_planner.cpp
index f2647803..9c584415 100644
--- a/noether_tpp/src/tool_path_planners/raster/plane_slicer_raster_planner.cpp
+++ b/noether_tpp/src/tool_path_planners/raster/plane_slicer_raster_planner.cpp
@@ -25,6 +25,7 @@
 #include <vtkPoints.h>
 #include <vtkPolyData.h>
 #include <vtkPolyDataNormals.h>
+#include <vtkGenericCell.h>
 #include <vtkSmartPointer.h>
 #include <vtkStripper.h>
 
@@ -338,7 +339,8 @@ noether::ToolPaths convertToPoses(const std::vector<RasterConstructData>& raster
 bool insertNormals(const double search_radius,
                    vtkSmartPointer<vtkPolyData>& mesh_data_,
                    vtkSmartPointer<vtkKdTreePointLocator>& kd_tree_,
-                   vtkSmartPointer<vtkPolyData>& data)
+                   vtkSmartPointer<vtkPolyData>& data,
+                   vtkSmartPointer<vtkCellLocator>& cell_locator_)
 {
   // Find closest cell to each point and uses its normal vector
   vtkSmartPointer<vtkDoubleArray> new_normals = vtkSmartPointer<vtkDoubleArray>::New();
@@ -358,39 +360,51 @@ bool insertNormals(const double search_radius,
   {
     // locate closest cell
     Eigen::Vector3d query_point;
-    vtkSmartPointer<vtkIdList> id_list = vtkSmartPointer<vtkIdList>::New();
     data->GetPoints()->GetPoint(i, query_point.data());
-    kd_tree_->FindPointsWithinRadius(search_radius, query_point.data(), id_list);
-    if (id_list->GetNumberOfIds() < 1)
-    {
-      kd_tree_->FindClosestNPoints(1, query_point.data(), id_list);
 
+    if (search_radius > 0.0)
+    {
+      vtkSmartPointer<vtkIdList> id_list = vtkSmartPointer<vtkIdList>::New();
+      kd_tree_->FindPointsWithinRadius(search_radius, query_point.data(), id_list);
       if (id_list->GetNumberOfIds() < 1)
       {
-        return false;
-      }
-    }
+        kd_tree_->FindClosestNPoints(1, query_point.data(), id_list);
 
-    // compute normal average
-    normal_vect = Eigen::Vector3d::Zero();
-    std::size_t num_normals = 0;
-    for (auto p = 0; p < id_list->GetNumberOfIds(); p++)
-    {
-      Eigen::Vector3d temp_normal, query_point, closest_point;
-      vtkIdType p_id = id_list->GetId(p);
+        if (id_list->GetNumberOfIds() < 1)
+        {
+          return false;
+        }
+      }
 
-      if (p_id < 0)
+      // compute normal average
+      normal_vect = Eigen::Vector3d::Zero();
+      std::size_t num_normals = 0;
+      for (auto p = 0; p < id_list->GetNumberOfIds(); p++)
       {
-        continue;
+        Eigen::Vector3d temp_normal, query_point, closest_point;
+        vtkIdType p_id = id_list->GetId(p);
+
+        if (p_id < 0)
+        {
+          continue;
+        }
+
+        // get normal and add it to average
+        normal_data->GetTuple(p_id, temp_normal.data());
+        normal_vect += temp_normal.normalized();
+        num_normals++;
       }
 
-      // get normal and add it to average
-      normal_data->GetTuple(p_id, temp_normal.data());
-      normal_vect += temp_normal.normalized();
-      num_normals++;
+      normal_vect /= num_normals;
+    }
+    else{
+      Eigen::Vector3d closest_point;
+      vtkIdType cellId;
+      int subid;
+      double dist2;
+      cell_locator_->FindClosestPoint(query_point.data(), closest_point.data(), cellId, subid, dist2);
+      mesh_data_->GetCellData()->GetNormals()->GetTuple(cellId, normal_vect.data());
     }
-
-    normal_vect /= num_normals;
     normal_vect.normalize();
 
     // save normal
@@ -615,7 +629,7 @@ ToolPaths PlaneSlicerRasterPlanner::planImpl(const pcl::PolygonMesh& mesh) const
         segment_data->SetPoints(new_points);
 
         // inserting normals
-        if (!insertNormals(search_radius_, mesh_data_, kd_tree_, segment_data))
+        if (!insertNormals(search_radius_, mesh_data_, kd_tree_, segment_data, cell_locator_))
         {
           throw std::runtime_error("Could not insert normals for segment " + std::to_string(r.raster_segments.size()) +
                                    " of raster " + std::to_string(i));

From aa4dc500c237b73a968c489ba1f7912e23055dc9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?I=C3=B1igo=20Moreno?= <inigo.moreno@tecnalia.com>
Date: Wed, 24 Jan 2024 14:05:57 +0100
Subject: [PATCH 2/2] Pass clang format

---
 .../tool_path_planners/raster/plane_slicer_raster_planner.cpp  | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/noether_tpp/src/tool_path_planners/raster/plane_slicer_raster_planner.cpp b/noether_tpp/src/tool_path_planners/raster/plane_slicer_raster_planner.cpp
index 9c584415..da2310c6 100644
--- a/noether_tpp/src/tool_path_planners/raster/plane_slicer_raster_planner.cpp
+++ b/noether_tpp/src/tool_path_planners/raster/plane_slicer_raster_planner.cpp
@@ -397,7 +397,8 @@ bool insertNormals(const double search_radius,
 
       normal_vect /= num_normals;
     }
-    else{
+    else
+    {
       Eigen::Vector3d closest_point;
       vtkIdType cellId;
       int subid;