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

GVD lib formatted

parent 956e7106
#include "GVD.h"
// A* single
/// euclidean distance heuristic
......
......@@ -5,14 +5,14 @@
#include <boost/type.hpp>
#include <iostream>
#include <boost/graph/astar_search.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/astar_search.hpp>
#include <math.h>
#include <cfloat>
#include "pos.h"
#include "grid.h"
#include "pos.h"
#include "utils.h"
......@@ -37,7 +37,7 @@ struct gvd_vertex {
}
};
template<typename graph>
template <typename graph>
struct genericGraph {
typedef graph Graph;
typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
......@@ -48,7 +48,7 @@ struct genericGraph {
typedef typename graph_traits<Graph>::vertex_iterator VertexIterator;
typedef typename graph_traits<Graph>::adjacency_iterator AdjacencyIterator;
typedef typename graph_traits<Graph>::edge_iterator EdgeIterator;
typedef typename graph_traits<Graph>::edge_iterator EdgeIterator;
typedef typename graph_traits<Graph>::out_edge_iterator OutEdgeIterator;
// typedef graph_traits<Graph>::adjacency_iterator adjacency_iterator;
......@@ -63,13 +63,9 @@ struct genericGraph {
return add_edge(u, v, w, g);
}
pair<Edge, bool> add_e(pos v,pos u){
return add_e(positions[v],positions[u]);
}
pair<Edge, bool> add_e(pos v, pos u) { return add_e(positions[v], positions[u]); }
void erase_e(pos v,pos u){
return remove_edge(positions[v],positions[u],g);
}
void erase_e(pos v, pos u) { return remove_edge(positions[v], positions[u], g); }
boost::tuple<Vertex, bool> add_v(pos p) {
NameVertexMapIterator pos_it;
......@@ -85,45 +81,37 @@ struct genericGraph {
}
return boost::make_tuple(u, inserted);
}
void erase_v(pos p){
void erase_v(pos p) {
if (positions.find(p) != positions.end()) {
Vertex cv = positions[p];
clear_vertex(cv, g);
remove_vertex(cv, g);
positions.erase(p);
remove_vertex(cv, g);
positions.erase(p);
}
}
void erase_v(pos_set to_erase){
for(auto it = to_erase.begin(); it != to_erase.end(); it++){
void erase_v(pos_set to_erase) {
for (auto it = to_erase.begin(); it != to_erase.end(); it++) {
erase_v(*it);
}
}
gvd_vertex operator[](pos p){
return g[positions[p]];
}
gvd_vertex operator[](Vertex v){
return g[v];
}
gvd_vertex operator[](pos p) { return g[positions[p]]; }
std::pair<AdjacencyIterator,AdjacencyIterator> neighbors(pos p){
return adjacent_vertices(positions[p],g);
}
std::pair<AdjacencyIterator,AdjacencyIterator> vs(){
return vertices(g);
}
int deg(pos p){
return degree(positions[p],g);
gvd_vertex operator[](Vertex v) { return g[v]; }
std::pair<AdjacencyIterator, AdjacencyIterator> neighbors(pos p) {
return adjacent_vertices(positions[p], g);
}
std::pair<AdjacencyIterator, AdjacencyIterator> vs() { return vertices(g); }
int deg(pos p) { return degree(positions[p], g); }
};
template<typename graph>
struct uncollapsedGraph : genericGraph<graph>{
template <typename graph>
struct uncollapsedGraph : genericGraph<graph> {
typedef graph Graph;
typedef genericGraph<Graph> GGraph;
......@@ -140,18 +128,18 @@ struct uncollapsedGraph : genericGraph<graph>{
uncollapsedGraph(){};
void add_v(pos_set to_add){
for(auto it = to_add.begin(); it != to_add.end(); it++){
void add_v(pos_set to_add) {
for (auto it = to_add.begin(); it != to_add.end(); it++) {
pos p = *it;
Vertex pv;
bool inserted;
// add v to graph
boost::tie(pv, inserted) = GGraph::add_v(p);
// check for neighbors
for(auto it_n = neighbor_displacement.begin(); it_n != neighbor_displacement.end(); it_n++){
for (auto it_n = neighbor_displacement.begin(); it_n != neighbor_displacement.end(); it_n++) {
pos n = p + (*it_n);
// add edge
if(to_add.find(n) != to_add.end() || this->positions.find(n) != this->positions.end()){
if (to_add.find(n) != to_add.end() || this->positions.find(n) != this->positions.end()) {
Vertex nv;
boost::tie(nv, inserted) = GGraph::add_v(n);
GGraph::add_e(nv, pv);
......@@ -162,8 +150,8 @@ struct uncollapsedGraph : genericGraph<graph>{
}
};
template<typename graph>
struct collapsedGraph : genericGraph<graph>{
template <typename graph>
struct collapsedGraph : genericGraph<graph> {
typedef genericGraph<graph> super;
typedef graph Graph;
......@@ -178,116 +166,108 @@ struct collapsedGraph : genericGraph<graph>{
typedef typename graph_traits<Graph>::edge_iterator EdgeIterator;
typedef typename graph_traits<Graph>::out_edge_iterator OutEdgeIterator;
//syncronized structure (collapse gvd needs uncollapse to add and remove vertexs)
// syncronized structure (collapse gvd needs uncollapse to add and remove vertexs)
boost::unordered_set<pos> full_graph;
// vertex is uncollapsed
bool is_uncollapsed(pos p){
return is_elem(this->positions,p);
}
bool is_uncollapsed(pos p) { return is_elem(this->positions, p); }
// vertex is collapsed
bool is_collapsed(pos p){
return is_elem(full_graph, p) && !is_elem(this->positions,p);
}
bool is_collapsed(pos p) { return is_elem(full_graph, p) && !is_elem(this->positions, p); }
//TODO
bool is_collapsable(pos p){
return true;//!is_min(p) && deg(p) == 2;
}
bool is_uncollapsable(pos p){
return !is_collapsable(p);
// TODO
bool is_collapsable(pos p) {
return true; //! is_min(p) && deg(p) == 2;
}
bool is_uncollapsable(pos p) { return !is_collapsable(p); }
// get
pos get_uncollapsed_neighbor(pos p, pos dir){
for(p+=dir;is_collapsed(p);p+=dir);
pos get_uncollapsed_neighbor(pos p, pos dir) {
for (p += dir; !is_uncollapsed(p); p += dir);
return p;
}
// precond: p is collapsed
// from: a---------b
// to: a----p----b
void uncollapse(pos p){
super::add_v(p);
void uncollapse(pos p) {
super::add_v(p);
// check for neighbors
for(auto it = neighbor_displacement.begin(); it != neighbor_displacement.end();it++){
for (auto it = neighbor_displacement.begin(); it != neighbor_displacement.end(); it++) {
pos dir = *it;
// if valid direction
if(is_elem(full_graph,p + dir)){
pos n = get_uncollapsed_neighbor(p,dir);
if (is_elem(full_graph, p + dir)) {
pos n = get_uncollapsed_neighbor(p, dir);
//erase old edge
AdjacencyIterator it,it_end;
for (boost::tie(it,it_end) = super::neighbors(n); it!=it_end; it++){
// erase old edge
AdjacencyIterator it, it_end;
for (boost::tie(it, it_end) = super::neighbors(n); it != it_end; it++) {
pos nn = this->g[*it].p;
// if p and the "neighbor of the neighbor of p" have the same dir and sense
if(angle(p-n) == angle(nn-n)){
super::erase_e(n,nn);
super::add_e(n,p);
if (angle(p - n) == angle(nn - n)) {
super::erase_e(n, nn);
super::add_e(n, p);
break;
}
}
super::add_e(p,n);
super::add_e(p, n);
}
}
}
//TODO
void collapse(pos p){
// TODO
void collapse(pos p) {
pos_set adj = all_adj(p);
for(auto it = adj.begin(); it != adj.end();it++){
//if()
for (auto it = adj.begin(); it != adj.end(); it++) {
// if()
}
}
// uncollapse all collapsed of neighbors p
// force: true -> does not check conditions
// force: false -> checks conditions
void uncollapse_neighbors(pos p, bool force=false){
void uncollapse_neighbors(pos p, bool force = false) {
pos_set adj = all_adj(p);
for(auto it = adj.begin(); it != adj.end(); it++){
for (auto it = adj.begin(); it != adj.end(); it++) {
pos n = *it;
if(is_collapsed(n) && (force || is_uncollapsable(p))){
if (is_collapsed(n) && (force || is_uncollapsable(p))) {
uncollapse(n);
}
}
}
// collapse neighbors of p IF POSSIBLE
void collapse_neighbors(pos p){
void collapse_neighbors(pos p) {
pos_set adj = all_adj(p);
for(auto it = adj.begin(); it != adj.end(); it++){
for (auto it = adj.begin(); it != adj.end(); it++) {
pos n = *it;
if(is_collapsable(p)){
if (is_collapsable(p)) {
collapse(n);
}
}
}
void erase_v(pos p){
void erase_v(pos p) {
// 0. erase from uncollapsed
full_graph.erase(p);
// 1. erase from collapsed
if(is_uncollapsed(p)){
if (is_uncollapsed(p)) {
super::erase_v(p);
// 1.1. collapse neighbors of p if possible
collapse_neighbors(p);
}
// 2. uncolloapse collapsed neighbors of p
uncollapse_neighbors(p);
}
void add_v(pos p){
if(is_elem(full_graph,p)){
void add_v(pos p) {
if (is_elem(full_graph, p)) {
/// update local min
return;
}
......@@ -297,11 +277,10 @@ struct collapsedGraph : genericGraph<graph>{
// TODO may not be true (adding an aleardy added)
// 1. uncolloapse collapsed neighbors of p
uncollapse_neighbors(p,true);
uncollapse_neighbors(p, true);
// 2. connect all the neighbors of p
// 3. collapse p if possible
collapse(p);
......@@ -309,28 +288,36 @@ struct collapsedGraph : genericGraph<graph>{
collapse_neighbors(p);
}
void add_v(pos_set to_add){
for(auto it = to_add.begin(); it != to_add.end(); it++){
add_v(*it);
void add_v(pos_set to_add) {
for (auto it = to_add.begin(); it != to_add.end(); it++) {
pos p = *it;
//add_v(*it); TODO descomentar esto y borrar lo de abajo (lo hice para probar)
super::add_v(p);
full_graph.insert(p);
}
}
};
// cuidado con el uso dehash_setS
typedef genericGraph<adjacency_list<listS, listS, bidirectionalS, gvd_vertex,property<edge_weight_t, float>>> GVD;
typedef genericGraph<adjacency_list<vecS, vecS, bidirectionalS, gvd_vertex,property<edge_weight_t, float>>> VecGVD;
typedef uncollapsedGraph<adjacency_list<listS, listS, bidirectionalS, gvd_vertex,property<edge_weight_t, float>>> uncollapsedGVD;
typedef collapsedGraph<adjacency_list<listS, listS, bidirectionalS, gvd_vertex,property<edge_weight_t, float>>> collapsedGVD;
typedef genericGraph<
adjacency_list<listS, listS, bidirectionalS, gvd_vertex, property<edge_weight_t, float>>>
GVD;
typedef genericGraph<
adjacency_list<vecS, vecS, bidirectionalS, gvd_vertex, property<edge_weight_t, float>>>
VecGVD;
typedef uncollapsedGraph<
adjacency_list<listS, listS, bidirectionalS, gvd_vertex, property<edge_weight_t, float>>>
uncollapsedGVD;
typedef collapsedGraph<
adjacency_list<listS, listS, bidirectionalS, gvd_vertex, property<edge_weight_t, float>>>
collapsedGVD;
using namespace std;
boost::tuple<list<VecGVD::Vertex>,float> get_single_path(VecGVD gvd, pos from, pos to);
boost::tuple<boost::unordered_map<pos,list<VecGVD::Vertex>> , boost::unordered_map<pos,float>> get_multi_path(VecGVD gvd, pos start, pos_set goals);
boost::tuple<list<VecGVD::Vertex>, float> get_single_path(VecGVD gvd, pos from, pos to);
boost::tuple<boost::unordered_map<pos, list<VecGVD::Vertex>>, boost::unordered_map<pos, float>>
get_multi_path(VecGVD gvd, pos start, pos_set goals);
#endif
......@@ -60,7 +60,7 @@ void print(string s) {
* main funcs
*/
GVD generate_uncollapsedGVD(grid_gvd ggvd){
GVD generate_uncollapsedGVD(grid_gvd ggvd) {
pair<int, int> size = get_grid_size(ggvd);
GVD gvd;
// initialize grid_gvd
......@@ -95,7 +95,6 @@ GVD generate_uncollapsedGVD(grid_gvd ggvd){
return gvd;
};
vector<pos> neighbor = {pos(-1, -1), pos(-1, 0), pos(-1, 1), pos(0, 1),
pos(1, 1), pos(1, 0), pos(1, -1), pos(0, -1)};
......@@ -351,8 +350,6 @@ boost::unordered_map<pos, bool> get_local_mins(dist_grid dg, GVD& gvd) {
return lmins;
}
void collapse_vertices(GVD& gvd, boost::unordered_map<pos, bool> lmins) {
for (auto vp = vertices(gvd.g); vp.first != vp.second;) {
auto vp_aux = vp.first;
......@@ -623,4 +620,3 @@ boost::tuple<criticals_info, GVD> get_points_of_interest(grid_type ogrid) {
cout << "debug :: make_tuple" << endl;
return boost::make_tuple(cis, gvd);
}
......@@ -4,20 +4,18 @@
#include <bits/stdc++.h>
#include <iostream>
#include <boost/graph/astar_search.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/astar_search.hpp>
#include <math.h>
#include <cfloat>
#include "GVD.h"
#include "pos.h"
#include "grid.h"
#include "critical_info.h"
#include "grid.h"
#include "pos.h"
using namespace std;
/*
* dist cell, col and grid
*/
......@@ -38,7 +36,6 @@ struct dist_cell {
typedef vector<dist_cell> dist_row;
typedef vector<dist_row> dist_grid;
using namespace std;
/*
......@@ -46,14 +43,15 @@ using namespace std;
*/
boost::tuple<dist_grid, dist_pos_queue> calculate_distances(grid_type ogrid, cell_type from_type);
boost::tuple<boost::unordered_map<pos,pos>, pos> find_paths_to_gvd(grid_type ogrid, VecGVD gvd, pos p_pos);
boost::tuple<boost::unordered_map<pos, pos>, pos> find_paths_to_gvd(grid_type ogrid,
VecGVD gvd,
pos p_pos);
grid_gvd get_grid_gvd(dist_grid dg, dist_pos_queue);
//boost::unordered_map<pos, dist_pos> get_critical_points(grid_type ogrid, dist_grid dg, GVD& gvd);
// boost::unordered_map<pos, dist_pos> get_critical_points(grid_type ogrid, dist_grid dg, GVD& gvd);
// boost::unordered_map<pos,bool> get_local_mins(dist_grid dg, GVD gvd);
boost::tuple<criticals_info, GVD> get_points_of_interest(grid_type g);
#endif
\ No newline at end of file
#include "GVDIC.h"
//////////
// AUX
//////////// returns the set of closest obstacles and the distance to them
boost::tuple<float, vector<pos>>
closest_obstacles(pos p, pos_set obsts) {
boost::tuple<float, vector<pos>> closest_obstacles(pos p, pos_set obsts) {
float d = MAXFLOAT;
vector<pos> closest_obsts;
for (auto it = obsts.begin(); it != obsts.end(); it++) {
......@@ -25,10 +23,10 @@ closest_obstacles(pos p, pos_set obsts) {
// recheck obstacles of n removing the invalid ones O(1) as you can only
// have up to 8 equidistant obstacles in a 2D grid
bool GVDIC::is_occ(pos_set &obsts){
for(auto it = obsts.begin(); it != obsts.end();){
bool GVDIC::is_occ(pos_set& obsts) {
for (auto it = obsts.begin(); it != obsts.end();) {
pos obst = (*it);
if(gt[obst.first][obst.second] != Occupied){
if (gt[obst.first][obst.second] != Occupied) {
it = obsts.erase(it);
} else {
++it;
......@@ -46,7 +44,7 @@ GVDIC::CellDataO::CellDataO() {
is_voro = false;
}
void GVDIC::CellDataO::clear_cell(){
void GVDIC::CellDataO::clear_cell() {
dist = MAXFLOAT;
is_cleared = true;
parents.clear();
......@@ -62,7 +60,7 @@ GVDIC::CellDataC::CellDataC() {
to_raise = false;
}
void GVDIC::CellDataC::clear_cell(){
void GVDIC::CellDataC::clear_cell() {
dist = MAXFLOAT;
is_possible_crit = false;
is_cleared = true;
......@@ -70,59 +68,58 @@ void GVDIC::CellDataC::clear_cell(){
obsts.clear();
}
/////////////////////////////
// UPDATE DISTANCE MAP
///////////////////////////
void GVDIC::set_voro(bool value, pos n){
CellDataO &n_cd = cell_data_o[n];
if(!value && !n_cd.is_voro)
void GVDIC::set_voro(bool value, pos n) {
CellDataO& n_cd = cell_data_o[n];
if (!value && !n_cd.is_voro)
return;
//cout<<"Celda: "<<n<<" valor: "<<value<<endl;
if(value){
// cout<<"Celda: "<<n<<" valor: "<<value<<endl;
if (value) {
voro_to_true.insert(n);
voro_to_false.erase(n);// TODO maybe there is a more efficient way to control the changes
}else{
voro_to_false.erase(n); // TODO maybe there is a more efficient way to control the changes
} else {
voro_to_false.insert(n);
voro_to_true.erase(n);// TODO maybe there is a more efficient way to control the changes
voro_to_true.erase(n); // TODO maybe there is a more efficient way to control the changes
}
n_cd.is_voro = value;
}
void GVDIC::check_voro(pos s, pos n){
CellDataO &s_cd = cell_data_o[s];
CellDataO &n_cd = cell_data_o[n];
if(n_cd.obsts.size()>1){
void GVDIC::check_voro(pos s, pos n) {
CellDataO& s_cd = cell_data_o[s];
CellDataO& n_cd = cell_data_o[n];
if (n_cd.obsts.size() > 1) {
set_voro(true, n);
}
if(s_cd.obsts.size()>1){
if (s_cd.obsts.size() > 1) {
set_voro(true, s);
}
pos obst_n = *n_cd.obsts.begin();
pos obst_s = *s_cd.obsts.begin();
// lo de adjacente no se fija que sea mayor a o la cant de obst de s
auto adj_n = adj(gt, obst_n);
if(is_occ(n_cd.obsts) && (obst_n != obst_s) && !is_elem(adj_n,obst_s)){
if((dist(s, obst_n) <= dist(n, obst_s)) && s_cd.dist > 0){
if (is_occ(n_cd.obsts) && (obst_n != obst_s) && !is_elem(adj_n, obst_s)) {
if ((dist(s, obst_n) <= dist(n, obst_s)) && s_cd.dist > 0) {
set_voro(true, s);
}
if((dist(n, obst_s) <= dist(s, obst_n)) && n_cd.dist > 0){
if ((dist(n, obst_s) <= dist(s, obst_n)) && n_cd.dist > 0) {
set_voro(true, n);
}
}
}
void GVDIC::process_lower(pos s) {
CellDataO &s_cd = cell_data_o[s];
CellDataO& s_cd = cell_data_o[s];
bool novr = false;//near_other_voronoi_region;
bool novr = false; // near_other_voronoi_region;
pos_set adj_s = adj(gt, s);
for (auto it = adj_s.begin(); it != adj_s.end(); it++) {
pos n = (*it);
CellDataO &n_cd = cell_data_o[n];
CellDataO& n_cd = cell_data_o[n];
// optimization avoids unnecesary operations
if (!n_cd.to_raise) {
float d;
......@@ -130,7 +127,7 @@ void GVDIC::process_lower(pos s) {
boost::tie(d, s_closest_obsts) = closest_obstacles(n, s_cd.obsts);
// cout<<n<<" d "<< d <<" dist "<< n_cd.dist <<endl;
if (d < n_cd.dist) {
//cout << "lowered: " <<n << endl;
// cout << "lowered: " <<n << endl;
n_cd.dist = d;
// cout<<n<<"updated dist to "<< n_cd.dist <<endl;
n_cd.parents.clear();
......@@ -144,9 +141,9 @@ void GVDIC::process_lower(pos s) {
n_cd.obsts.insert(s_closest_obsts.begin(), s_closest_obsts.end());
n_cd.parents.insert(s);
}
if(is_occ(n_cd.obsts)){
check_voro(s,n);
//cout << "check_voro" << n << endl;
if (is_occ(n_cd.obsts)) {
check_voro(s, n);
// cout << "check_voro" << n << endl;
}
}
}
......@@ -154,7 +151,7 @@ void GVDIC::process_lower(pos s) {
}
void GVDIC::process_raise(pos s) {
CellDataO &s_cd = cell_data_o[s];
CellDataO& s_cd = cell_data_o[s];
float min_d = MAXFLOAT;
pos_set s_closest_obsts;
vector<pos> s_parents;
......@@ -162,17 +159,17 @@ void GVDIC::process_raise(pos s) {
pos_set adj_s = adj(gt, s);
for (auto it = adj_s.begin(); it != adj_s.end(); it++) {
pos n = (*it);
CellDataO &n_cd = cell_data_o[n];
CellDataO& n_cd = cell_data_o[n];
if (!n_cd.is_cleared && !n_cd.to_raise) {
// if n does not have any valid obstacle then its distance is invalid
// it and should propagate the process raise
if(!is_occ(n_cd.obsts)){