to test

src/lib/GVD/DisMapIC.cpp

@@ -19,51 +19,47 @@ void DisMapIC::CellData::clear_cell() {
   sources.clear();
 }
 
-// returns the set of closest sources and the distance to them
-boost::tuple<float, vector<pos>> closest_sources(pos p, pos_set sources) {
-  float d = MAXFLOAT;
-  vector<pos> closest_sources;
-  for (auto it = sources.begin(); it != sources.end(); it++) {
-    pos source = *it;
-    float d_to_source = dist(p, source);
-    if (d > d_to_source) {
-      d = d_to_source;
-      closest_sources.clear();
-      closest_sources.push_back(source);
-    } else if (d == d_to_source) {
-      closest_sources.push_back(source);
-    }
-  }
-  return boost::make_tuple(d, closest_sources);
-}
-
 // recheck sources of n removing the invalid ones O(1) as you can only
 // have up to 8 equidistant sources in a 2D grid
-bool DisMapIC::valid(pos_set& sources) {
+void DisMapIC::clean_invalid_sources(pos_set& sources) {
   for (auto it = sources.begin(); it != sources.end();) {
     pos source = (*it);
-    if ((*m)[source] != source_type) {
+    if (cell_data[source].dist!=0) {
       it = sources.erase(it);
     } else {
       ++it;
     }
   }
-  return sources.size() > 0;
 }
 
-void DisMapIC::process_lower(pos s) {
-  CellData& s_cd = cell_data[s];
+/* returns true only if the cd has a valid source and
+   has a parent thats also has a parent.
+   reason: not taking the next ring of a raise as valid */
+bool DisMapIC::valid(CellData cd){
+  clean_invalid_sources(cd.sources);
+  if(cd.sources.empty() || cd.parents.empty()){
+    return false;
+  }
+  for(auto it = cd.parents.begin(); it != cd.parents.end(); it++){
+    pos n = *it;
+    if(!cell_data[n].parents.empty()){
+     return true;
+    }
+  }
+  return false;
+}
+
+void DisMapIC::process_lower(pos p) {
+  CellData& p_cd = cell_data[p];
 
-  pos_set adj_s = m->adj(s,obstructed_types);
-  for (auto it = adj_s.begin(); it != adj_s.end(); it++) {
+  pos_set adj_p = m->adj(p,obstructed_types);
+  for (auto it = adj_p.begin(); it != adj_p.end(); it++) {
     pos n = (*it);
     CellData& n_cd = cell_data[n];
     // optimization avoids unnecesary operations
     if (!n_cd.to_raise) {
-      float d;
-      vector<pos> s_closest_sources;
-      boost::tie(d, s_closest_sources) = closest_sources(n, s_cd.sources);
       // cout<<n<<" d "<< d <<" dist "<< n_cd.dist <<endl;
+      float d = dist(n,p) + p_cd.dist;
       if (d < n_cd.dist) {
         // cout << "lowered: " <<n << endl;
         n_cd.dist = d;
@@ -71,87 +67,79 @@ void DisMapIC::process_lower(pos s) {
         n_cd.parents.clear();
         n_cd.parents.insert(n);
         n_cd.sources.clear();
-        n_cd.sources.insert(s_closest_sources.begin(), s_closest_sources.end());
+        n_cd.sources.insert(p_cd.sources.begin(), p_cd.sources.end());
         n_cd.is_cleared = false;
         open.push(make_pair(d, n));
-      } else {
-        if (d == n_cd.dist) {
-          n_cd.parents.insert(n);
-          n_cd.sources.insert(s_closest_sources.begin(), s_closest_sources.end());
-        }
-        if (valid(n_cd.sources)) {
-          check_voro(s, n);
-          // cout << "check_voro" << n << endl;
-        }
+      } else if (d == n_cd.dist) {
+        n_cd.parents.insert(n);
+        n_cd.sources.insert(p_cd.sources.begin(), p_cd.sources.end());
       }
     }
   }
 }
 
-void DisMapIC::process_raise(pos s) {
-  CellData& s_cd = cell_data[s];
+void DisMapIC::process_raise(pos p) {
+  CellData& p_cd = cell_data[p];
   float min_d = MAXFLOAT;
-  pos_set s_closest_sources;
-  vector<pos> s_parents;
-  // for each neighbor n of s
-  pos_set adj_s = m->adj(s,obstructed_types);
-  for (auto it = adj_s.begin(); it != adj_s.end(); it++) {
+  pos_set p_closest_sources;
+  vector<pos> p_parents;
+  // for each neighbor n of p
+  pos_set adj_p = m->adj(p,obstructed_types);
+  for (auto it = adj_p.begin(); it != adj_p.end(); it++) {
     pos n = (*it);
     CellData& n_cd = cell_data[n];
 
     if (!n_cd.is_cleared && !n_cd.to_raise) {
-      // if n does not have any valid sourceacle then its distance is invalid
-      // it and should propagate the process raise
-      if (!valid(n_cd.sources)) {
+      // if n is not valid it should propagate the process raise
+      n_cd.parents.erase(p);
+      if (!valid(n_cd)) {
         // cout << "raised: " <<n << endl;
         open.push(make_pair(n_cd.dist, n));
         n_cd.clear_cell();
         n_cd.to_raise = true;
       } else {
-        // if n has a valid sourceacle then it could be used
-        // to obtain the disstance of s
-        float d;
-        vector<pos> n_closest_sources;
-        boost::tie(d, n_closest_sources) = closest_sources(s, n_cd.sources);
+        // if is valid then it could be used
+        // to obtain the disstance of p
+        float d = dist(p,n) + n_cd.dist;
         // cout<<n<<" d "<< d <<" dist "<< n_cd.dist <<endl;
         if (d < min_d) {
           min_d = d;
           // cout<<n<<"updated dist to "<< n_cd.dist <<endl;
-          s_parents.clear();
-          s_parents.push_back(n);
-          s_closest_sources.clear();
-          s_closest_sources.insert(n_closest_sources.begin(), n_closest_sources.end());
+          p_parents.clear();
+          p_parents.push_back(n);
+          p_closest_sources.clear();
+          p_closest_sources.insert(n_cd.sources.begin(), n_cd.sources.end());
         } else if (d == min_d) {
-          s_closest_sources.insert(n_closest_sources.begin(), n_closest_sources.end());
-          s_parents.push_back(n);
+          p_closest_sources.insert(n_cd.sources.begin(), n_cd.sources.end());
+          p_parents.push_back(n);
         }
       }
     }
   }
 
   if (min_d < MAXFLOAT) {
-    s_cd.dist = min_d;
-    s_cd.parents.clear();
-    s_cd.parents.insert(s_closest_sources.begin(), s_closest_sources.end());
-    s_cd.sources.clear();
-    s_cd.sources.insert(s_closest_sources.begin(), s_closest_sources.end());
-    s_cd.is_cleared = false;
-    open.push(make_pair(s_cd.dist, s));
+    p_cd.dist = min_d;
+    p_cd.parents.clear();
+    p_cd.parents.insert(p_closest_sources.begin(), p_closest_sources.end());
+    p_cd.sources.clear();
+    p_cd.sources.insert(p_closest_sources.begin(), p_closest_sources.end());
+    p_cd.is_cleared = false;
+    open.push(make_pair(p_cd.dist, p));
   }
 
-  s_cd.to_raise = false;
+  p_cd.to_raise = false;
 }
 
-void DisMapIC::set_consistent_borders(vector<pos> unk_to_free) {
+void DisMapIC::set_consistent_borders(vector<pos> obstructed_to_free) {
   pos_set inserted;
-  for (int i = 0; i < unk_to_free.size(); i++) {
-    pos s = unk_to_free[i];
-    // cout << "unk_to_free"<<s << endl;
-    pos_set adj_s = m->adj(s,obstructed_types);
-    for (auto it = adj_s.begin(); it != adj_s.end(); it++) {
+  for (int i = 0; i < obstructed_to_free.size(); i++) {
+    pos p = obstructed_to_free[i];
+    // cout << "unk_to_free"<<p << endl;
+    pos_set adj_p = m->adj(p,obstructed_types);
+    for (auto it = adj_p.begin(); it != adj_p.end(); it++) {
       pos n = (*it);
       CellData& n_cd = cell_data[n];
-      if (valid(n_cd.sources) && !is_elem(inserted, s)) {
+      if (valid(n_cd) && !is_elem(inserted, p)) {
         // cout << " border:"<<n << endl;
         open.push(make_pair(n_cd.dist, n));
         inserted.insert(n);
@@ -162,6 +150,31 @@ void DisMapIC::set_consistent_borders(vector<pos> unk_to_free) {
   print_property(inserted, m->g);
 }
 
+void DisMapIC::set_source(pos p) {
+  CellData& p_cd = cell_data[p];
+  p_cd.dist = 0;
+  p_cd.parents.clear();
+  p_cd.parents.insert(p);
+  p_cd.sources.clear();
+  p_cd.sources.insert(p);
+  p_cd.is_cleared = false;
+  open.push(make_pair(0, p));
+}
+
+void DisMapIC::remove_source(pos p) {
+  CellData& p_cd = cell_data[p];
+  p_cd.clear_cell();
+  p_cd.to_raise = true;
+  open.push(make_pair(0, p));
+}
+
+void DisMapIC::obstruct(pos p) {
+  CellData& p_cd = cell_data[p];
+  open.push(make_pair(p_cd.dist, p));
+  p_cd.clear_cell();
+  p_cd.to_raise = true;
+}
+
 void DisMapIC::update_distance_map(vector<pos> source_to_true,
                                      vector<pos> source_to_false,
                                      vector<pos> obstructed_to_free,
@@ -174,26 +187,37 @@ void DisMapIC::update_distance_map(vector<pos> source_to_true,
     // cout << "Remove an source source" << obs_to_free[i] << endl;
   }
 
-  set_consistent_borders(obstructed_to_free);
+  for (int i = 0; i < free_to_obstructed.size(); i++) {
+    obstruct(free_to_obstructed[i]);
+    // cout << "Set an source    " << any_to_obs[i] << endl;
+  }
 
+  // maybe some borders are unnecesary 
+  set_consistent_borders(obstructed_to_free);
+  
   for (int i = 0; i < source_to_true.size(); i++) {
     set_source(source_to_true[i]);
     // cout << "Set an source    " << any_to_obs[i] << endl;
   }
 
   // cout<<endl<<"# Rebuild GVD"<<endl;
+
+  raise_and_lower();
+}
+
+void DisMapIC::raise_and_lower(){
   while (!open.empty()) {
-    pos s = open.top().second;
+    pos p = open.top().second;
     open.pop();
 
-    CellData& s_cd = cell_data[s];
+    CellData& p_cd = cell_data[p];
 
-    if (s_cd.to_raise) {
-      // cout << "Process RAISE    "<<s << endl;
-      process_raise(s);
-    } else if (valid(s_cd.sources)) {
-      // cout << "Process LOWER    "<<s << endl;
-      process_lower(s);
+    if (p_cd.to_raise) {
+      // cout << "Process RAISE    "<<p << endl;
+      process_raise(p);
+    } else if (valid(p_cd)) {
+      // cout << "Process LOWER    "<<p << endl;
+      process_lower(p);
     }
   }
 }
\ No newline at end of file

src/lib/GVD/DisMapIC.h

@@ -27,20 +27,25 @@ class DisMapIC{
   CellDataPQ open;
 
   // functions
-  bool valid(pos_set& sources);
+  bool valid(CellData cd);
+  void clean_invalid_sources(pos_set& sources);
   void check_voro(pos s, pos n);
 
   void set_source(pos p);
   void remove_source(pos p);
+  void obstruct(pos p);
+  void unobstruct(pos p);
   void set_consistent_borders(vector<pos>);
 
   void process_lower(pos p);
   void process_raise(pos p);
 
+  // if p source->obstructed then p in source_to_false
   void update_distance_map(vector<pos> source_to_true,
                            vector<pos> source_to_false,
                            vector<pos> obstructed_to_free,
                            vector<pos> free_to_obstructed);
+  void raise_and_lower();
 
   cell_type source_type;
   vector<cell_type> obstructed_types;

src/lib/GVD/Map.cpp

@@ -13,7 +13,7 @@ void Map::update(grid g) {
   int mapHeight = g[0].size();
   for (int x = 0; x < mapWidth; x++) {
     for (int y = 0; y < mapHeight; y++) {
-      this->changes.insert(make_pair(pos(x,y),g[x][y]);
+      this->changes.insert(make_pair(pos(x,y),g[x][y]));
     }
   }
 }