no map

src/lib/GVD/.gitignore

@@ -7,3 +7,4 @@ compile_commands.json
 build
 CTestTestfile.cmake
 libMAIN_SOURCES.a
+test

src/lib/GVD/GVDIC.cpp

@@ -2,8 +2,9 @@
 
 //////////
 // AUX
+//////////
 
-//////////// returns the set of closest obstacles and the distance to them
+// returns the set of closest obstacles and the distance to them
 boost::tuple<float, vector<pos>> closest_obstacles(pos p, pos_set obsts) {
   float d = MAXFLOAT;
   vector<pos> closest_obsts;

src/lib/GVD/IDisMapGen.cpp

+#include "IDisMapGen.h"
+
+IDisMapGen::IDisMapGen(vector<grid_type> obstructed_types, grid_type source_type){
+    this->obstructed_types = obstructed_types;
+    this->source_type = source_type;
+  };
+
+// CellData
+IDisMapGen::CellData::CellData() {
+  dist = MAXFLOAT;
+  is_cleared = true;
+  to_raise = false;
+}
+
+void IDisMapGen::CellData::clear_cell() {
+  dist = MAXFLOAT;
+  is_cleared = true;
+  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 IDisMapGen::valid(pos_set& sources) {
+  for (auto it = sources.begin(); it != sources.end();) {
+    pos source = (*it);
+    if (gt[source.first][source.second] != Occupied) {
+      it = sources.erase(it);
+    } else {
+      ++it;
+    }
+  }
+  return sources.size() > 0;
+}
+
+void IDisMapGen::process_lower(pos s) {
+  CellData& s_cd = cell_data[s];
+
+  pos_set adj_s = adj(gt, s);
+  for (auto it = adj_s.begin(); it != adj_s.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;
+      if (d < n_cd.dist) {
+        // cout << "lowered: " <<n << endl;
+        n_cd.dist = d;
+        // cout<<n<<"updated dist to "<< n_cd.dist <<endl;
+        n_cd.sources.clear();
+        n_cd.sources.insert(s_closest_sources.begin(), s_closest_sources.end());
+        n_cd.is_cleared = false;
+        open.push(make_pair(d, n));
+      } else {
+        if (d == n_cd.dist) {
+          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;
+        }
+      }
+    }
+  }
+}
+
+void IDisMapGen::process_raise(pos s) {
+  CellData& s_cd = cell_data[s];
+  float min_d = MAXFLOAT;
+  pos_set s_closest_sources;
+  vector<pos> s_parents;
+  // for each neighbor n of s
+  pos_set adj_s = adj(gt, s);
+  for (auto it = adj_s.begin(); it != adj_s.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)) {
+        // 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);
+        // 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(s);
+          s_closest_sources.clear();
+          s_closest_sources.insert(n_closest_sources.begin(), n_closest_sources.end());
+        } else if (d == min_d) {
+          s_closest_sources.insert(n_closest_sources.begin(), n_closest_sources.end());
+          s_parents.push_back(n);
+        }
+      }
+    }
+  }
+
+  if (min_d < MAXFLOAT) {
+    s_cd.dist = min_d;
+    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));
+  }
+
+  s_cd.to_raise = false;
+}
+
+void IDisMapGen::update_distance_map(vector<pos> obs_to_free,
+                                vector<pos> any_to_obs,
+                                vector<pos> unk_to_free,
+                                vector<pos> unk_to_notunk) {
+  // cout<<endl<<"# Getting changes"<<endl;
+
+  // cout<<endl<<"# setting cd for the changes"<<endl;
+  for (int i = 0; i < obs_to_free.size(); i++) {
+    remove_source(obs_to_free[i]);
+    // cout << "Remove an sourceacle " << obs_to_free[i] << endl;
+  }
+
+  set_consistent_borders(unk_to_free);
+
+  for (int i = 0; i < any_to_obs.size(); i++) {
+    set_source(any_to_obs[i]);
+    // cout << "Set an sourceacle    " << any_to_obs[i] << endl;
+  }
+
+  // cout<<endl<<"# Rebuild GVD"<<endl;
+  while (!open.empty()) {
+    pos s = open.top().second;
+    open.pop();
+
+    CellData& s_cd = cell_data[s];
+
+    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);
+    }
+  }
+}
+

src/lib/GVD/IDisMapGen.h

+#pragma once
+
+#include "grid.h"
+#include "pos.h"
+#include "utils.h"
+
+class IDisMapGen{
+ private:
+#define NULL_POS pos(MAXFLOAT, MAXFLOAT)
+
+  struct CellData {
+    float dist;
+    pos_set sources;
+
+    bool to_raise;
+
+    bool is_cleared;
+
+    CellData();
+    void clear_cell();
+  };
+
+  grid_type old_gt;
+  grid_type gt;
+
+  typedef priority_queue<pair<float, pos>, vector<pair<float, pos>>, greater<pair<float, pos>>>
+      CellDataPQ;
+  CellDataPQ open;
+
+  // functions
+  bool valid(pos_set& sources);
+  void check_voro(pos s, pos n);
+
+  void set_source(pos p);
+  void remove_source(pos p);
+  void set_consistent_borders(vector<pos>);
+
+  void process_lower(pos p);
+  void process_raise(pos p);
+
+  void update_distance_map(vector<pos> obs_to_free,
+                           vector<pos> any_to_obs,
+                           vector<pos> unk_to_free,
+                           vector<pos> unk_to_notunk);
+
+  // voro
+  void set_voro(bool value, pos n);
+
+  grid_type source_type;
+  vector<grid_type> obstructed_types;
+
+ public:
+  int DEBUG = 0;
+
+  boost::unordered_map<pos, CellData> cell_data;
+
+  pos_set voro_to_false;
+  pos_set voro_to_true;
+
+  IDisMapGen(vector<grid_type> obstructed_types, grid_type source_type);
+};

src/lib/GVD/Map.h

+#pragma once

src/lib/GVD/grid.h

@@ -54,12 +54,16 @@ static vector<pos> neighbor_displacement = {pos(-1, -1), pos(-1, 0), pos(-1, 1),
                                             pos(1, 1),   pos(1, 0),  pos(1, -1), pos(0, -1)};
 
 // returns all *known* neighbors of a given pos p in the grid gt
-inline pos_set adj(grid_type gt, pos p) {
+inline pos_set adj(grid_type gt, pos p, vector<cell_type> invalid_types = {Unknown}) {
   pos_set adj;
   for (auto it = neighbor_displacement.begin(); it != neighbor_displacement.end(); it++) {
     pos a = p + (*it);
     if (a > pos(-1, -1) && a < pos(gt.size(), gt[0].size())) {
-      if (gt[a.first][a.second] != Unknown) {
+      bool valid = true;
+      for(auto it = invalid_types.begin(); valid && it!=invalid_types.end(); it++){
+        valid = cell(gt,a) != *it;
+      }
+      if (valid) {
         adj.insert(a);
       }
     }
@@ -87,4 +91,4 @@ inline pos_set all_adj(pos p) {
     adj.insert(a);
   }
   return adj;
-}
\ No newline at end of file
+}