Commit c5646a01 authored by Federico Ciuffardi's avatar Federico Ciuffardi
Browse files

bug fix on check_voro condition

parent 8ba93364
......@@ -303,6 +303,7 @@ struct collapsedGraph : genericGraph<graph> {
//cout<<"ENTRE AL ERASE CON ALGO MAL: "<<p<<endl;
return;
}
// cout<<"erasing "<<p<<endl;
//cout<<"ENTRO AL ERASE V CON P: "<<p<<endl;
// 0. erase from uncollapsed
full_graph.erase(p);
......@@ -345,6 +346,7 @@ struct collapsedGraph : genericGraph<graph> {
//cout<<"ENTRE AL ADD CON ALGO MAL"<<endl;
return;
}
// cout<<"adding "<<p<<endl;
// 0. uncolloapse collapsed neighbors of p
//cout<<"PASO 0 CON P: "<<p<<endl;
......
......@@ -121,7 +121,7 @@ void GVDIC::CellDataO::clear_cell() {
//adds cell n to a set depending on value.
//this fuction builds sets of cells to add, to delete and to update the GVD
void GVDIC::set_voro(bool value, pos n) {
/* cout<<"sv "<<n<<" : "<<value<<endl; */
// cout<<"sv "<<n<<" : "<<value<<endl;
CellDataO& n_cd = cell_data_o[n];
// cout<<"Celda: "<<n<<" valor: "<<value<<endl;
if (value) {
......@@ -159,15 +159,30 @@ void GVDIC::check_voro(pos s, pos n) {
if (s_cd.obsts.size() > 1) {
set_voro(true, s);
}
pos obst_n = *n_cd.obsts.begin();
pos obst_s = *s_cd.obsts.begin();
auto adj_n = adj(gt, obst_n);
// pos obst_n = *n_cd.obsts.begin();
// pos obst_s = *s_cd.obsts.begin();
// auto adj_n = adj(gt, obst_n);
//cout<<"n "<<n<<"| s "<<s<<endl;
//cout<<" (is_occ(n_cd,n) && (obst_n != obst_s) && !is_elem(adj_n, obst_s))"<<endl;
//cout<<" "<<is_occ(n_cd,n) << " " << (obst_n != obst_s) <<" "<< !is_elem(adj_n, obst_s)<<endl; //beforej
if (is_occ(n_cd,n) && (n_cd.obsts != s_cd.obsts)) {
// skip if all obstacles from s are adjacent to one or more obstacle from n
bool are_adj = true;
for(auto it =s_cd.obsts.begin(); it != s_cd.obsts.end() && are_adj;it++){
pos obst_s = *it;
for(auto it =n_cd.obsts.begin(); it != n_cd.obsts.end() && are_adj;it++){
pos obst_n = *it;
auto adj_n = adj(gt, obst_n);
are_adj = is_elem(adj_n,obst_s);
}
}
if(are_adj) return;
if (is_occ(n_cd,n) && (obst_n != obst_s) && !is_elem(adj_n, obst_s)) {
if ((dist(s, obst_n) <= dist(n, obst_s)) && s_cd.dist > 0) {
if (s_cd.dist > 0) { //(dist(s, obst_n) <= dist(n, obst_s)) && could add the multi obstacle variation
set_voro(true, s);
}
if ((dist(n, obst_s) <= dist(s, obst_n)) && n_cd.dist > 0) {
if (n_cd.dist > 0) { //(dist(n, obst_s) <= dist(s, obst_n)) && could add the multi obstacle variation
set_voro(true, n);
}
}
......@@ -478,25 +493,27 @@ boost::tuple<pos_set, pos_set> GVDIC::erosion(pos_set erosion_input) {
pos_set adj_p = adj(gt, p);
for (auto it = adj_p.begin(); it != adj_p.end(); it++) {
pos n = *it;
if (!is_elem(inserted, n) && cell_data_o[n].is_voro) {
if (!is_elem(inserted, n) && cell_data_o[n].is_voro) {
inserted.insert(n);
erosion_queue.push(make_pair(cell_data_o[n].dist, n));
}
}
}
// cout<<"erosion"<<endl;
// print_property(inserted,gt);
//cout<<"erosion"<<endl;
//print_property(inserted,gt);
while (!erosion_queue.empty()) {
pos p = erosion_queue.top().second;
erosion_queue.pop();
// cout<<p<<" obsts "<<cell_data_o[p].obsts.size() <<"| A " <<A(p) <<endl;
if (cell_data_o[p].obsts.size() < 2 && A(p) <= 1) {
// cout<<p<<"->eroded"<<endl;
cell_data_o[p].is_voro = false;
if(is_elem(gvd.full_graph, p)){
distance_update.erase(p);
to_erase.insert(p);
}
} else {
// cout<<p<<"->inserted"<<endl;
//if is !in full
if (!is_elem(gvd.full_graph, p)){
to_add.insert(p);
......@@ -528,10 +545,16 @@ void GVDIC::update_gvd() {
pos_set to_add_gvd, to_erase_gvd, to_update_gvd, to_update_min, erosion_input;
// erosion input
erosion_input.insert(obst_cond_to_false.begin(),obst_cond_to_false.end());
erosion_input.insert(voro_to_true.begin(), voro_to_true.end());
erosion_input.insert(distance_update.begin(), distance_update.end());
// as old_region_border cells must go from true to false (and not otherwise) we filter out the true ones
/// as old_region_border and obst_cond_to_false cells must go from true to false
/// (and not otherwise) we filter out the true ones
for(auto it = obst_cond_to_false.begin(); it != obst_cond_to_false.end(); it++){
pos p = *it;
if(cell_data_o[p].is_voro){
erosion_input.insert(p);
}
}
for(auto it = old_region_border.begin(); it != old_region_border.end(); it++){
pos p = *it;
if(cell_data_o[p].is_voro){
......@@ -553,14 +576,14 @@ void GVDIC::update_gvd() {
// print_property(voro_to_true,gt);
// cout<<"old_region_border"<<endl;
// print_property(old_region_border,gt);
//cout<<"voro_to_false"<<endl;
// cout<<"voro_to_false"<<endl;
// print_property(voro_to_false,gt);
// cout<<"distance_update"<<endl;
// print_property(distance_update,gt);
// cout<<"obst_cond_to_false"<<endl;
// print_property(obst_cond_to_false,gt);
//cout<<"erosion_input"<<endl;
//print_property(erosion_input,gt);
// cout<<"erosion_input"<<endl;
// print_property(erosion_input,gt);
// erosion
boost::tie(to_add_gvd, to_erase_gvd) = erosion(erosion_input);
......
......@@ -422,40 +422,40 @@ vector<grid> test_9 = {
vector<grid> test_10 = {
{
{Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
{Free , Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Free , Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Free , Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Free , Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Free , Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
{Unknown, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
{Unknown, Free , Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Unknown, Free , Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Unknown, Free , Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Unknown, Free , Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Unknown, Free , Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Unknown, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
},
{
{Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
{Free , Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Free , Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Free , Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Free , Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Free , Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
{Unknown, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
{Unknown, Free , Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Unknown, Free , Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Unknown, Free , Free , Free , Free , Free , Free , Free , Free ,Free , Free },
{Unknown, Free , Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Unknown, Free , Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Unknown, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
},
{
{Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
{Free , Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Free , Free , Occupied, Free , Free , Occupied, Free , Free ,Free , Free },
{Free , Free , Occupied, Free , Free , Free , Free , Free ,Free , Free },
{Free , Free , Occupied, Free , Free , Occupied, Free , Free ,Free , Free },
{Free , Free , Occupied, Free , Free , Occupied, Free , Free ,Free , Free },
{Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
{Unknown, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
{Unknown, Free , Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Unknown, Free , Free , Occupied, Free , Free , Occupied, Free , Free ,Free , Free },
{Unknown, Free , Free , Occupied, Free , Free , Free , Free , Free ,Free , Free },
{Unknown, Free , Free , Occupied, Free , Free , Occupied, Free , Free ,Free , Free },
{Unknown, Free , Free , Occupied, Free , Free , Occupied, Free , Free ,Free , Free },
{Unknown, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
},
{
{Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
{Free , Free , Occupied, Free , Free , Occupied, Free , Free ,Free , Free },
{Free , Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Free , Free , Occupied, Free , Free , Occupied, Free , Free ,Free , Free },
{Free , Free , Occupied, Free , Free , Occupied, Free , Free ,Free , Free },
{Free , Free , Occupied, Free , Free , Occupied, Free , Free ,Free , Free },
{Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
{Unknown, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
{Unknown, Free , Free , Occupied, Free , Free , Occupied, Free , Free ,Free , Free },
{Unknown, Free , Free , Free , Free , Free , Occupied, Free , Free ,Free , Free },
{Unknown, Free , Free , Occupied, Free , Free , Occupied, Free , Free ,Free , Free },
{Unknown, Free , Free , Occupied, Free , Free , Occupied, Free , Free ,Free , Free },
{Unknown, Free , Free , Occupied, Free , Free , Occupied, Free , Free ,Free , Free },
{Unknown, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied, Occupied,Occupied, Occupied},
},
};
vector<vector<grid>> tests = {test_1, test_2, test_3, test_4, test_5, test_6, test_7, test_8,test_9,test_10};
......@@ -491,7 +491,14 @@ int main(int argc, char** argv) {
GVDIC igvdc;
cout << "# INITIALIZATION DONE" << endl;
// cout<<"DIST_NEW: "<< igvdc.cell_data_o[pos(0,0)].dist << endl;
for (int i = 0; i < test[0].size(); i++) {
for (int j = 0; j < test[0][i].size(); j++) {
pos p = pos(i, j);
cout << "|" << std::setfill(' ') << std::setw(7) << p;
}
cout << "|" << endl;
}
cout << endl;
for (int t = 0; t < test.size(); t++) {
cout << endl << "# get_points_of_interest " << t << endl;
igvdc.get_points_of_interest(test[t]);
......@@ -579,19 +586,19 @@ int main(int argc, char** argv) {
cout << "|" << endl;
}
// cout << endl << "# Posible Critics distance map " << t << endl;
// for (int i = 0; i < test[t].size(); i++) {
// for (int j = 0; j < test[t][i].size(); j++) {
// float d = igvdc.disMapIC->cell_data[pos(i, j)].dist;
// if (d == MAXFLOAT) {
// cout << "| ∞";
// } else {
// cout << "|" << std::setfill(' ') << std::setw(7) << d;
// }
// }
// cout << "|" << endl;
// }
// cout << endl;
// }
cout << endl << "# Posible Critics distance map " << t << endl;
for (int i = 0; i < test[t].size(); i++) {
for (int j = 0; j < test[t][i].size(); j++) {
float d = igvdc.disMapIC->cell_data[pos(i, j)].dist;
if (d == MAXFLOAT) {
cout << "| ∞";
} else {
cout << "|" << std::setfill(' ') << std::setw(7) << d;
}
}
cout << "|" << endl;
}
cout << endl;
}
return 0;
}
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