Commit 1864284c authored by Federico Ciuffardi's avatar Federico Ciuffardi Committed by Romina Julieta Parada Venossa
Browse files

no map

parent 604a5677
......@@ -7,3 +7,4 @@ compile_commands.json
build
CTestTestfile.cmake
libMAIN_SOURCES.a
test
......@@ -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;
......
#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);
}
}
}
#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);
};
......@@ -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
}
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