Commit 63c35f91 authored by Federico Ciuffardi's avatar Federico Ciuffardi Committed by Romina Julieta Parada Venossa
Browse files

to test

parent 2fa22114
......@@ -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
......@@ -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;
......
......@@ -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]));
}
}
}
......
No preview for this file type
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment