From 5ccf470fe242b8ac352a2cde60486476cb806f9c Mon Sep 17 00:00:00 2001
From: Marco Livesu <marco.livesu@gmail.com>
Date: Wed, 27 Mar 2024 16:35:16 +0100
Subject: [PATCH] dijkstra_exhaustive_mask_on_edges

---
 include/cinolib/dijkstra.cpp | 42 ++++++++++++++++++++++++++++++++++++
 include/cinolib/dijkstra.h   |  9 ++++++++
 2 files changed, 51 insertions(+)

diff --git a/include/cinolib/dijkstra.cpp b/include/cinolib/dijkstra.cpp
index 6f27d8df..69e5bc0f 100644
--- a/include/cinolib/dijkstra.cpp
+++ b/include/cinolib/dijkstra.cpp
@@ -181,6 +181,48 @@ void dijkstra_exhaustive_srf_only(const AbstractPolyhedralMesh<M,V,E,F,P> & m,
 
 //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
 
+template<class M, class V, class E, class P>
+CINO_INLINE
+void dijkstra_exhaustive_mask_on_edges(const AbstractMesh<M,V,E,P> & m,
+                                       const uint                  & source,
+                                       const std::vector<bool>     & mask,    // if mask[e] = true, path cannot pass through edge e
+                                             std::vector<double>   & dist)
+{
+    dist = std::vector<double>(m.num_verts(), inf_double);
+    dist.at(source) = 0.0;
+
+    std::set<std::pair<double,uint>> q;
+    q.insert(std::make_pair(0.0,source));
+
+    while(!q.empty())
+    {
+        uint vid = q.begin()->second;
+        q.erase(q.begin());
+
+        for(uint eid : m.adj_v2e(vid))
+        {
+            if(mask.at(eid)) continue;
+
+            uint   nbr      = m.vert_opposite_to(eid,vid);
+            double new_dist = dist.at(vid) + m.edge_length(eid);
+
+            if(dist.at(nbr) > new_dist)
+            {
+                if(dist.at(nbr) < inf_double) // otherwise it won't be found (one order of magnitude faster than initializing the queue with all the elements with inf dist)
+                {
+                    auto it = q.find(std::make_pair(dist.at(nbr),nbr));
+                    assert(it!=q.end());
+                    q.erase(it);
+                }
+                dist.at(nbr) = new_dist;
+                q.insert(std::make_pair(new_dist,nbr));
+            }
+        }
+    }
+}
+
+//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
+
 template<class M, class V, class E, class P>
 CINO_INLINE
 void dijkstra_exhaustive_mask_on_edges(const AbstractMesh<M,V,E,P> & m,
diff --git a/include/cinolib/dijkstra.h b/include/cinolib/dijkstra.h
index e9e14879..b04fb9fa 100644
--- a/include/cinolib/dijkstra.h
+++ b/include/cinolib/dijkstra.h
@@ -74,6 +74,15 @@ void dijkstra_exhaustive_srf_only(const AbstractPolyhedralMesh<M,V,E,F,P> & m,
 
 //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
 
+template<class M, class V, class E, class P>
+CINO_INLINE
+void dijkstra_exhaustive_mask_on_edges(const AbstractMesh<M,V,E,P> & m,
+                                       const uint                  & source,
+                                       const std::vector<bool>     & mask,    // if mask[e] = true, path cannot pass through edge e
+                                             std::vector<double>   & dist);
+
+//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
+
 template<class M, class V, class E, class P>
 CINO_INLINE
 void dijkstra_exhaustive_mask_on_edges(const AbstractMesh<M,V,E,P> & m,