Use edge length from mesh in `igl::dijkstra`. (#1170)

* Add new (and sane) overload to Dijkstra

* Add a test for Dijkstra

* Dijkstra: template instantiation for static build, and small fixes

include/igl/dijkstra.cpp

@@ -79,8 +79,59 @@ IGL_INLINE void igl::dijkstra(
     path.push_back(source);
 }
 
+
+template <typename IndexType, typename DerivedV,
+typename DerivedD, typename DerivedP>
+IGL_INLINE int igl::dijkstra(
+  const Eigen::MatrixBase<DerivedV> &V,
+  const std::vector<std::vector<IndexType> >& VV,
+  const IndexType &source,
+  const std::set<IndexType> &targets,
+  Eigen::PlainObjectBase<DerivedD> &min_distance,
+  Eigen::PlainObjectBase<DerivedP> &previous)
+{
+  int numV = VV.size();
+
+  min_distance.setConstant(numV, 1, std::numeric_limits<typename DerivedD::Scalar>::infinity());
+  min_distance[source] = 0;
+  previous.setConstant(numV, 1, -1);
+  std::set<std::pair<typename DerivedD::Scalar, IndexType> > vertex_queue;
+  vertex_queue.insert(std::make_pair(min_distance[source], source));
+
+  while (!vertex_queue.empty())
+  {
+    typename DerivedD::Scalar dist = vertex_queue.begin()->first;
+    IndexType u = vertex_queue.begin()->second;
+    vertex_queue.erase(vertex_queue.begin());
+
+    if (targets.find(u)!= targets.end())
+      return u;
+
+    // Visit each edge exiting u
+    const std::vector<int> &neighbors = VV[u];
+    for (std::vector<int>::const_iterator neighbor_iter = neighbors.begin();
+         neighbor_iter != neighbors.end();
+         neighbor_iter++)
+    {
+      IndexType v = *neighbor_iter;
+      typename DerivedD::Scalar distance_through_u = dist + (V.row(u) - V.row(v)).norm();
+      if (distance_through_u < min_distance[v]) {
+        vertex_queue.erase(std::make_pair(min_distance[v], v));
+
+        min_distance[v] = distance_through_u;
+        previous[v] = u;
+        vertex_queue.insert(std::make_pair(min_distance[v], v));
+
+      }
+
+    }
+  }
+  return -1;
+}
+
 #ifdef IGL_STATIC_LIBRARY
 // Explicit template instantiation
 template int igl::dijkstra<int, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<int, -1, 1, 0, -1, 1> >(int const&, std::set<int, std::less<int>, std::allocator<int> > const&, std::vector<std::vector<int, std::allocator<int> >, std::allocator<std::vector<int, std::allocator<int> > > > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&);
 template void igl::dijkstra<int, Eigen::Matrix<int, -1, 1, 0, -1, 1> >(int const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, std::vector<int, std::allocator<int> >&);
+template int igl::dijkstra<int, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<int, -1, 1, 0, -1, 1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, std::vector<std::vector<int, std::allocator<int> >, std::allocator<std::vector<int, std::allocator<int> > > > const&, int const&, std::set<int, std::less<int>, std::allocator<int> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&);
 #endif

include/igl/dijkstra.h

@@ -74,9 +74,33 @@ namespace igl {
     const IndexType &vertex,
     const Eigen::MatrixBase<DerivedP> &previous,
     std::vector<IndexType> &path);
-};
 
 
+  // Dijkstra's algorithm for shortest paths on a mesh, with multiple targets, using edge length
+  //
+  // Inputs:
+  //   V                #V by 3 list of vertex positions
+  //   VV               #V list of lists of incident vertices (adjacency list), e.g.
+  //                    as returned by igl::adjacency_list, will be generated if empty.
+  //   source           index of source vertex
+  //   targets          target vector set
+  //
+  // Output:
+  //   min_distance     #V by 1 list of the minimum distances from source to all vertices
+  //   previous         #V by 1 list of the previous visited vertices (for each vertex) - used for backtracking
+  //
+  template <typename IndexType, typename DerivedV,
+  typename DerivedD, typename DerivedP>
+  IGL_INLINE int dijkstra(
+    const Eigen::MatrixBase<DerivedV> &V,
+    const std::vector<std::vector<IndexType> >& VV,
+    const IndexType &source,
+    const std::set<IndexType> &targets,
+    Eigen::PlainObjectBase<DerivedD> &min_distance,
+    Eigen::PlainObjectBase<DerivedP> &previous);
+
+}
+
 #ifndef IGL_STATIC_LIBRARY
 #include "dijkstra.cpp"
 #endif

tests/include/igl/dijkstra.cpp

@@ -0,0 +1,22 @@
+#include <test_common.h>
+#include <igl/dijkstra.h>
+#include <igl/adjacency_list.h>
+#include <iostream>
+
+TEST_CASE("dijkstra: cube", "[igl]")
+{
+  Eigen::MatrixXd V;
+  Eigen::MatrixXi F;
+  //This is a cube of dimensions 1.0x1.0x1.0
+  test_common::load_mesh("cube.off", V, F);
+
+  std::vector<std::vector<int>> VV;
+  igl::adjacency_list(F, VV);
+
+  Eigen::VectorXd min_distance;
+  Eigen::VectorXi previous;
+  igl::dijkstra(V, VV, 0, {7}, min_distance, previous);
+
+  REQUIRE(min_distance(0) == 0);
+  REQUIRE(min_distance(7) == Approx(sqrt(2)).margin(1e-10));
+}