Merge pull request #36 from nilsbore/normals_at_points

Added function for getting normals at a set of points on the mesh
nilsbore · Sep 6, 2019 · a7967de · a7967de
2 parents b4b5b67 + e855723
commit a7967de
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 0 deletions.
diff --git a/src/bathy_maps/include/bathy_maps/mesh_map.h b/src/bathy_maps/include/bathy_maps/mesh_map.h
@@ -48,6 +48,7 @@ namespace mesh_map {
     std::tuple<Eigen::MatrixXd, Eigen::MatrixXi, Eigen::MatrixXd, BoundsT> mesh_and_normals_from_pings(const std_data::mbes_ping::PingsT& pings, double res);
     Eigen::MatrixXd shade_image_from_normals(const Eigen::MatrixXd& N, const BoundsT& bounds, double res, const Eigen::Vector3d& light_dir);
     Eigen::MatrixXd compute_normals(const Eigen::MatrixXd& V, const Eigen::MatrixXi& F);
+    Eigen::MatrixXd normals_at_points(const Eigen::MatrixXd& points, const Eigen::MatrixXd& N, const BoundsT& bounds, double res);
 
     void write_dae_mesh(const Eigen::MatrixXd& V, const Eigen::MatrixXi& F, const boost::filesystem::path& filename);
     void write_dae_mesh_from_str(const Eigen::MatrixXd& V, const Eigen::MatrixXi& F, const std::string& filename);

diff --git a/src/bathy_maps/src/mesh_map.cpp b/src/bathy_maps/src/mesh_map.cpp
@@ -526,4 +526,24 @@ Eigen::MatrixXd shade_image_from_normals(const Eigen::MatrixXd& N, const BoundsT
     return shade_image;
 }
 
+Eigen::MatrixXd normals_at_points(const Eigen::MatrixXd& points, const Eigen::MatrixXd& N, const BoundsT& bounds, double res)
+{
+    int cols = std::ceil((bounds(1, 0) - bounds(0, 0)) / res); // min/max should be in the center
+    int rows = std::ceil((bounds(1, 1) - bounds(0, 1)) / res);
+
+    cout << "Rows: " << rows << ", Cols: " << cols << endl;
+
+    Eigen::MatrixXd N_points = Eigen::MatrixXd::Zero(points.rows(), points.cols());
+
+    for (int i = 0; i < points.rows(); ++i) {
+        //int x = int((points(i, 0)-bounds(0, 0))/res);
+        //int y = int((points(i, 1)-bounds(0, 1))/res);
+        int x = int(points(i, 0)/res);
+        int y = int(points(i, 1)/res);
+        N_points.row(i) = N.row(y*cols+x);
+    }
+
+    return N_points;
+}
+
 } // namespace mesh_map
diff --git a/src/pybathy_maps/src/pymesh_map.cpp b/src/pybathy_maps/src/pymesh_map.cpp
@@ -40,5 +40,6 @@ PYBIND11_MODULE(mesh_map, m) {
     m.def("mesh_and_normals_from_pings", &mesh_map::mesh_and_normals_from_pings, "Get vertices, faces, normals and bounds form mbes_ping::PingsT");
     m.def("shade_image_from_normals", &mesh_map::shade_image_from_normals, "Compute [0, 1] shade image from normals and lighting direction");
     m.def("compute_normals", &mesh_map::compute_normals, "Compute normals from the mesh, per vertex");
+    m.def("normals_at_points", &mesh_map::normals_at_points, "Get the normals at a set of points on the mesh");
 
 }