Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
Federico Ciuffardi Alves
pgMappingCooperativo
Commits
9e3ac14a
Commit
9e3ac14a
authored
Oct 08, 2020
by
Federico Ciuffardi
Committed by
Romina Julieta Parada Venossa
Oct 17, 2020
Browse files
in progress
parent
1864284c
Changes
12
Hide whitespace changes
Inline
Side-by-side
src/lib/GVD/GVDC.cpp
View file @
9e3ac14a
...
...
@@ -100,7 +100,7 @@ vector<pos> neighbor = {pos(-1, -1), pos(-1, 0), pos(-1, 1), pos(0, 1),
/* returns the dist_grid corresponding to the original grid, relative to
* Occupide or Critical (from_type) */
boost
::
tuple
<
dist_grid
,
dist_pos_queue
>
calculate_distances
(
grid
_type
ogrid
,
cell_type
from_type
)
{
boost
::
tuple
<
dist_grid
,
dist_pos_queue
>
calculate_distances
(
grid
ogrid
,
cell_type
from_type
)
{
// get grid size
pair
<
int
,
int
>
size
=
get_grid_size
(
ogrid
);
...
...
@@ -182,7 +182,7 @@ boost::tuple<dist_grid, dist_pos_queue> calculate_distances(grid_type ogrid, cel
return
boost
::
make_tuple
(
dgrid
,
full_dqueue
);
}
boost
::
tuple
<
boost
::
unordered_map
<
pos
,
pos
>
,
pos
>
find_paths_to_gvd
(
grid
_type
ogrid
,
boost
::
tuple
<
boost
::
unordered_map
<
pos
,
pos
>
,
pos
>
find_paths_to_gvd
(
grid
ogrid
,
VecGVD
gvd
,
pos
p_pos
)
{
// get grid size
...
...
@@ -431,7 +431,7 @@ void clean_up(GVD& gvd, dist_grid dgrid, int min_deg) {
}
}
int
degree_constraint
(
grid
_type
&
ogrid
,
GVD
&
gvd
,
boost
::
unordered_map
<
pos
,
bool
>
local_mins
)
{
int
degree_constraint
(
grid
&
ogrid
,
GVD
&
gvd
,
boost
::
unordered_map
<
pos
,
bool
>
local_mins
)
{
int
criticals_count
=
0
;
pos
current_pos
;
for
(
auto
vp
=
vertices
(
gvd
.
g
);
vp
.
first
!=
vp
.
second
;
++
vp
.
first
)
{
...
...
@@ -466,7 +466,7 @@ int degree_constraint(grid_type& ogrid, GVD& gvd, boost::unordered_map<pos, bool
return
criticals_count
;
}
/*boost::unordered_map<pos, dist_pos> unknown_dist_constraint(grid
_type
ogrid, GVD& gvd, int
/*boost::unordered_map<pos, dist_pos> unknown_dist_constraint(grid ogrid, GVD& gvd, int
criticals_count) { dist_grid dgrid; dist_pos_queue dqueue; boost::unordered_map<pos, dist_pos>
critical_with_frontier; boost::tie(dgrid, dqueue) = calculate_distances(ogrid, Critical); while
(criticals_count > 0 && !dqueue.empty()) { dist_pos frontier_dp = dqueue.top(); dqueue.pop(); pos
...
...
@@ -485,7 +485,7 @@ critical_pos = cell(dgrid, frontier_dp.second).obs[0]; GVD::Vertex cv = gvd.posi
}*/
// Returns two maps : criticals -> frontiers, criticals-> min dis to frontier, segments gvd
criticals_info
unknown_dist_constraint2
(
grid
_type
ogrid
,
GVD
&
gvd
)
{
criticals_info
unknown_dist_constraint2
(
grid
ogrid
,
GVD
&
gvd
)
{
dist_grid
dgrid
;
dist_pos_queue
dqueue
;
criticals_info
res
;
...
...
@@ -532,7 +532,7 @@ criticals_info unknown_dist_constraint2(grid_type ogrid, GVD& gvd) {
return
res
;
}
criticals_info
get_critical_points
(
grid
_type
ogrid
,
dist_grid
dg
,
GVD
&
gvd
)
{
criticals_info
get_critical_points
(
grid
ogrid
,
dist_grid
dg
,
GVD
&
gvd
)
{
boost
::
unordered_map
<
pos
,
bool
>
local_mins
=
get_local_mins
(
dg
,
gvd
);
cout
<<
"debug :: clean_up"
<<
endl
;
// TODO clean_up and collapse_vertices can be merged into one function
...
...
@@ -557,7 +557,7 @@ criticals_info get_critical_points(grid_type ogrid, dist_grid dg, GVD& gvd) {
}
/*void print_grid(grid_gvd ggvd,
grid
_type
grid,
grid grid,
boost::unordered_map<pos, dist_pos> cf = boost::unordered_map<pos, dist_pos>(),
boost::unordered_map<pos, bool> frontier_aux = boost::unordered_map<pos, bool>()) {
ofstream outfile;
...
...
@@ -596,7 +596,7 @@ criticals_info get_critical_points(grid_type ogrid, dist_grid dg, GVD& gvd) {
// fclose(stdout);
}*/
boost
::
tuple
<
criticals_info
,
GVD
>
get_points_of_interest
(
grid
_type
ogrid
)
{
boost
::
tuple
<
criticals_info
,
GVD
>
get_points_of_interest
(
grid
ogrid
)
{
boost
::
unordered_set
<
pos
>
res
;
dist_grid
dgrid
;
dist_pos_queue
dqueue
;
...
...
src/lib/GVD/GVDC.h
View file @
9e3ac14a
...
...
@@ -41,17 +41,17 @@ using namespace std;
/*
* functions
*/
boost
::
tuple
<
dist_grid
,
dist_pos_queue
>
calculate_distances
(
grid
_type
ogrid
,
cell_type
from_type
);
boost
::
tuple
<
dist_grid
,
dist_pos_queue
>
calculate_distances
(
grid
ogrid
,
cell_type
from_type
);
boost
::
tuple
<
boost
::
unordered_map
<
pos
,
pos
>
,
pos
>
find_paths_to_gvd
(
grid
_type
ogrid
,
boost
::
tuple
<
boost
::
unordered_map
<
pos
,
pos
>
,
pos
>
find_paths_to_gvd
(
grid
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 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
);
boost
::
tuple
<
criticals_info
,
GVD
>
get_points_of_interest
(
grid
g
);
#endif
\ No newline at end of file
#endif
src/lib/GVD/GVDIC.cpp
View file @
9e3ac14a
...
...
@@ -218,8 +218,7 @@ void GVDIC::process_raise(pos s) {
void
GVDIC
::
update_distance_map
(
vector
<
pos
>
obs_to_free
,
vector
<
pos
>
any_to_obs
,
vector
<
pos
>
unk_to_free
,
vector
<
pos
>
unk_to_notunk
)
{
vector
<
pos
>
unk_to_free
)
{
// cout<<endl<<"# Getting changes"<<endl;
// cout<<endl<<"# setting cd for the changes"<<endl;
...
...
@@ -330,7 +329,7 @@ void GVDIC::remove_obstacle(pos p) {
open
.
push
(
make_pair
(
0
,
p
));
}
boost
::
tuple
<
vector
<
pos
>
,
vector
<
pos
>
,
vector
<
pos
>
,
vector
<
pos
>>
GVDIC
::
get_changes
(
grid
_type
gt
)
{
boost
::
tuple
<
vector
<
pos
>
,
vector
<
pos
>
,
vector
<
pos
>
,
vector
<
pos
>>
GVDIC
::
get_changes
(
grid
gt
)
{
int
mapWidth
=
gt
.
size
();
if
(
mapWidth
==
0
)
{
cout
<<
"PLEASE USE A NOT EMPTY MAP"
<<
endl
;
...
...
@@ -556,7 +555,7 @@ void GVDIC::update_possible_crit(pos_set to_add_gvd, pos_set to_erase_gvd) {
// MAIN
//////////
boost
::
tuple
<
criticals_info
,
GVD
>
GVDIC
::
get_points_of_interest
(
grid
_type
gt
)
{
boost
::
tuple
<
criticals_info
,
GVD
>
GVDIC
::
get_points_of_interest
(
grid
gt
)
{
// pre increment
voro_to_true
.
clear
();
voro_to_false
.
clear
();
...
...
@@ -573,7 +572,7 @@ boost::tuple<criticals_info, GVD> GVDIC::get_points_of_interest(grid_type gt) {
boost
::
tie
(
obs_to_free
,
any_to_obs
,
unk_to_free
,
unk_to_notunk
)
=
get_changes
(
gt
);
update_frontiers
(
unk_to_notunk
);
update_distance_map
(
obs_to_free
,
any_to_obs
,
unk_to_free
,
unk_to_notunk
);
update_distance_map
(
obs_to_free
,
any_to_obs
,
unk_to_free
);
update_gvd
();
// post increment
...
...
src/lib/GVD/GVDIC.h
View file @
9e3ac14a
...
...
@@ -39,8 +39,8 @@ class GVDIC {
void
clear_cell
();
};
grid
_type
old_gt
;
grid
_type
gt
;
grid
old_gt
;
grid
gt
;
pos_set
frontiers
;
boost
::
unordered_map
<
pos
,
int
>
know_neighbors
;
...
...
@@ -53,7 +53,7 @@ class GVDIC {
bool
is_occ
(
pos_set
&
obsts
);
void
check_voro
(
pos
s
,
pos
n
);
boost
::
tuple
<
vector
<
pos
>
,
vector
<
pos
>
,
vector
<
pos
>
,
vector
<
pos
>>
get_changes
(
grid
_type
gt
);
boost
::
tuple
<
vector
<
pos
>
,
vector
<
pos
>
,
vector
<
pos
>
,
vector
<
pos
>>
get_changes
(
grid
gt
);
void
set_obstacle
(
pos
p
);
void
remove_obstacle
(
pos
p
);
void
set_consistent_borders
(
vector
<
pos
>
);
...
...
@@ -65,8 +65,7 @@ class GVDIC {
void
update_distance_map
(
vector
<
pos
>
obs_to_free
,
vector
<
pos
>
any_to_obs
,
vector
<
pos
>
unk_to_free
,
vector
<
pos
>
unk_to_notunk
);
vector
<
pos
>
unk_to_free
);
void
update_gvd
();
boost
::
tuple
<
pos_set
,
pos_set
>
erosion
(
pos_set
);
...
...
@@ -87,7 +86,7 @@ class GVDIC {
boost
::
unordered_map
<
pos
,
CellDataO
>
cell_data_o
;
boost
::
unordered_map
<
pos
,
CellDataC
>
cell_data_c
;
boost
::
tuple
<
criticals_info
,
GVD
>
get_points_of_interest
(
grid
_type
gt
);
boost
::
tuple
<
criticals_info
,
GVD
>
get_points_of_interest
(
grid
gt
);
pos_set
voro_to_false
;
pos_set
voro_to_true
;
...
...
src/lib/GVD/IDisMapGen.cpp
View file @
9e3ac14a
#include "IDisMapGen.h"
IDisMapGen
::
IDisMapGen
(
vector
<
grid_type
>
obstructed_types
,
grid_type
source_type
){
this
->
obstructed_types
=
obstructed_types
;
this
->
source_type
=
source_type
;
};
IDisMapGen
::
IDisMapGen
(
Map
*
m
,
vector
<
cell_type
>
obstructed_types
,
cell_type
source_type
){
this
->
m
=
m
;
this
->
obstructed_types
=
obstructed_types
;
this
->
source_type
=
source_type
;
};
// CellData
IDisMapGen
::
CellData
::
CellData
()
{
...
...
@@ -41,7 +42,7 @@ boost::tuple<float, vector<pos>> closest_sources(pos p, pos_set sources) {
bool
IDisMapGen
::
valid
(
pos_set
&
sources
)
{
for
(
auto
it
=
sources
.
begin
();
it
!=
sources
.
end
();)
{
pos
source
=
(
*
it
);
if
(
gt
[
source
.
first
][
source
.
second
]
!=
Occupied
)
{
if
(
(
*
m
)
[
source
]
!=
source_type
)
{
it
=
sources
.
erase
(
it
);
}
else
{
++
it
;
...
...
@@ -53,7 +54,7 @@ bool IDisMapGen::valid(pos_set& sources) {
void
IDisMapGen
::
process_lower
(
pos
s
)
{
CellData
&
s_cd
=
cell_data
[
s
];
pos_set
adj_s
=
adj
(
gt
,
s
);
pos_set
adj_s
=
m
->
adj
(
s
,
obstructed_type
s
);
for
(
auto
it
=
adj_s
.
begin
();
it
!=
adj_s
.
end
();
it
++
)
{
pos
n
=
(
*
it
);
CellData
&
n_cd
=
cell_data
[
n
];
...
...
@@ -67,12 +68,15 @@ void IDisMapGen::process_lower(pos s) {
// cout << "lowered: " <<n << endl;
n_cd
.
dist
=
d
;
// cout<<n<<"updated dist to "<< n_cd.dist <<endl;
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
.
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
))
{
...
...
@@ -90,7 +94,7 @@ void IDisMapGen::process_raise(pos s) {
pos_set
s_closest_sources
;
vector
<
pos
>
s_parents
;
// for each neighbor n of s
pos_set
adj_s
=
adj
(
gt
,
s
);
pos_set
adj_s
=
m
->
adj
(
s
,
obstructed_type
s
);
for
(
auto
it
=
adj_s
.
begin
();
it
!=
adj_s
.
end
();
it
++
)
{
pos
n
=
(
*
it
);
CellData
&
n_cd
=
cell_data
[
n
];
...
...
@@ -114,7 +118,7 @@ void IDisMapGen::process_raise(pos s) {
min_d
=
d
;
// cout<<n<<"updated dist to "<< n_cd.dist <<endl;
s_parents
.
clear
();
s_parents
.
push_back
(
s
);
s_parents
.
push_back
(
n
);
s_closest_sources
.
clear
();
s_closest_sources
.
insert
(
n_closest_sources
.
begin
(),
n_closest_sources
.
end
());
}
else
if
(
d
==
min_d
)
{
...
...
@@ -127,6 +131,8 @@ void IDisMapGen::process_raise(pos s) {
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
;
...
...
@@ -136,23 +142,43 @@ void IDisMapGen::process_raise(pos 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
)
{
void
IDisMapGen
::
set_consistent_borders
(
vector
<
pos
>
unk_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
++
)
{
pos
n
=
(
*
it
);
CellData
&
n_cd
=
cell_data
[
n
];
if
(
valid
(
n_cd
.
sources
)
&&
!
is_elem
(
inserted
,
s
))
{
// cout << " border:"<<n << endl;
open
.
push
(
make_pair
(
n_cd
.
dist
,
n
));
inserted
.
insert
(
n
);
}
}
}
std
::
cout
<<
"consistent borders"
<<
endl
;
print_property
(
inserted
,
m
->
g
);
}
void
IDisMapGen
::
update_distance_map
(
vector
<
pos
>
source_to_true
,
vector
<
pos
>
source_to_false
,
vector
<
pos
>
obstructed_to_free
,
vector
<
pos
>
free_to_obstructed
){
// cout<<endl<<"# Getting changes"<<endl;
// cout<<endl<<"# setting cd for the changes"<<endl;
for
(
int
i
=
0
;
i
<
obs
_to_f
re
e
.
size
();
i
++
)
{
remove_source
(
obs
_to_f
re
e
[
i
]);
// cout << "Remove an source
acle
" << obs_to_free[i] << endl;
for
(
int
i
=
0
;
i
<
source
_to_f
als
e
.
size
();
i
++
)
{
remove_source
(
source
_to_f
als
e
[
i
]);
// cout << "Remove an source
source
" << obs_to_free[i] << endl;
}
set_consistent_borders
(
unk
_to_free
);
set_consistent_borders
(
obstructed
_to_free
);
for
(
int
i
=
0
;
i
<
any_to_obs
.
size
();
i
++
)
{
set_source
(
any_to_obs
[
i
]);
// cout << "Set an source
acle
" << any_to_obs[i] << endl;
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;
...
...
@@ -170,5 +196,4 @@ void IDisMapGen::update_distance_map(vector<pos> obs_to_free,
process_lower
(
s
);
}
}
}
}
\ No newline at end of file
src/lib/GVD/IDisMapGen.h
View file @
9e3ac14a
#pragma once
#include "Map.h"
#include "grid.h"
#include "pos.h"
#include "utils.h"
...
...
@@ -11,6 +12,7 @@ class IDisMapGen{
struct
CellData
{
float
dist
;
pos_set
sources
;
pos_set
parents
;
bool
to_raise
;
...
...
@@ -20,9 +22,6 @@ class IDisMapGen{
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
;
...
...
@@ -38,16 +37,15 @@ class IDisMapGen{
void
process_lower
(
pos
p
);
void
process_raise
(
pos
p
);
void
update_distance_map
(
vector
<
pos
>
obs
_to_
fre
e
,
vector
<
pos
>
any_to_obs
,
vector
<
pos
>
unk
_to_free
,
vector
<
pos
>
unk_to_notunk
);
void
update_distance_map
(
vector
<
pos
>
source
_to_
tru
e
,
vector
<
pos
>
source_to_false
,
vector
<
pos
>
obstructed
_to_free
,
vector
<
pos
>
free_to_obstructed
);
// voro
v
oid
set_voro
(
bool
value
,
pos
n
)
;
cell_type
source_type
;
v
ector
<
cell_type
>
obstructed_types
;
grid_type
source_type
;
vector
<
grid_type
>
obstructed_types
;
Map
*
m
=
NULL
;
public:
int
DEBUG
=
0
;
...
...
@@ -57,5 +55,5 @@ class IDisMapGen{
pos_set
voro_to_false
;
pos_set
voro_to_true
;
IDisMapGen
(
vector
<
grid
_type
>
obstructed_types
,
grid
_type
source_type
);
IDisMapGen
(
Map
*
m
,
vector
<
cell
_type
>
obstructed_types
,
cell
_type
source_type
);
};
src/lib/GVD/Map.cpp
View file @
9e3ac14a
#include "Map.h"
cell_type
&
Map
::
operator
[](
pos
p
){
return
cell
(
g
,
p
);
}
void
Map
::
update
(
grid
g
)
{
this
->
g
=
g
;
int
mapWidth
=
g
.
size
();
if
(
mapWidth
==
0
)
{
cout
<<
"PLEASE USE A NON EMPTY MAP"
<<
endl
;
}
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
]);
}
}
}
// returns all *known* neighbors of a given pos p in the grid g
pos_set
Map
::
adj
(
pos
p
,
vector
<
cell_type
>
obstructed_types
)
{
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
(
g
.
size
(),
g
[
0
].
size
()))
{
bool
valid
=
true
;
for
(
auto
it
=
obstructed_types
.
begin
();
valid
&&
it
!=
obstructed_types
.
end
();
it
++
){
valid
=
cell
(
g
,
a
)
!=
*
it
;
}
if
(
valid
)
{
adj
.
insert
(
a
);
}
}
}
return
adj
;
}
// returns ALL neighbors of a given pos p in the grid g
pos_set
Map
::
all_adj
(
pos
p
)
{
return
adj
(
p
,
{});
}
// returns ALL neighbors of a given pos p in the grid g
vector
<
pos
>
Map
::
dirs
()
{
return
neighbor_displacement
;
}
src/lib/GVD/Map.h
View file @
9e3ac14a
#pragma once
#include <vector>
#include "grid.h"
struct
Map
{
grid
g
;
set
<
pair
<
pos
,
cell_type
>>
changes
;
cell_type
&
operator
[](
pos
p
);
void
update
(
grid
g
);
pos_set
adj
(
pos
p
,
vector
<
cell_type
>
invalid_types
);
pos_set
all_adj
(
pos
p
);
vector
<
pos
>
dirs
();
};
src/lib/GVD/grid.h
View file @
9e3ac14a
...
...
@@ -15,7 +15,7 @@ using namespace std;
*/
enum
cell_type
{
Occupied
,
Unknown
,
Free
,
Critical
,
Frontier
};
typedef
vector
<
cell_type
>
row_type
;
typedef
vector
<
row_type
>
grid
_type
;
typedef
vector
<
row_type
>
grid
;
/*
* Misc
...
...
@@ -54,7 +54,7 @@ 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
,
vector
<
cell_type
>
invalid_types
=
{
Unknown
})
{
inline
pos_set
adj
(
grid
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
);
...
...
@@ -72,7 +72,7 @@ inline pos_set adj(grid_type gt, pos p, vector<cell_type> invalid_types = {Unkno
}
// returns ALL neighbors of a given pos p in the grid gt
inline
pos_set
all_adj
(
grid
_type
gt
,
pos
p
)
{
inline
pos_set
all_adj
(
grid
gt
,
pos
p
)
{
pos_set
adj
;
for
(
auto
it
=
neighbor_displacement
.
begin
();
it
!=
neighbor_displacement
.
end
();
it
++
)
{
pos
a
=
p
+
(
*
it
);
...
...
src/lib/GVD/test
0 → 100755
View file @
9e3ac14a
File added
src/lib/GVD/test.cpp
View file @
9e3ac14a
...
...
@@ -3,24 +3,24 @@
using
namespace
std
;
grid
_type
grid1
=
{{
Occupied
,
Occupied
,
Occupied
,
Occupied
},
grid
grid1
=
{{
Occupied
,
Occupied
,
Occupied
,
Occupied
},
{
Occupied
,
Free
,
Free
,
Occupied
},
{
Occupied
,
Free
,
Free
,
Occupied
},
{
Occupied
,
Occupied
,
Occupied
,
Occupied
}};
grid
_type
grid2
=
{{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
},
grid
grid2
=
{{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
},
{
Occupied
,
Free
,
Free
,
Free
,
Occupied
},
{
Occupied
,
Free
,
Free
,
Free
,
Occupied
},
{
Occupied
,
Free
,
Free
,
Free
,
Occupied
},
{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
}};
grid
_type
grid3
=
{{
Occupied
,
Occupied
,
Occupied
,
Occupied
},
grid
grid3
=
{{
Occupied
,
Occupied
,
Occupied
,
Occupied
},
{
Occupied
,
Free
,
Free
,
Occupied
},
{
Occupied
,
Free
,
Free
,
Occupied
},
{
Occupied
,
Free
,
Free
,
Occupied
},
{
Occupied
,
Occupied
,
Occupied
,
Occupied
}};
grid
_type
grid4
=
{{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
},
grid
grid4
=
{{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
},
{
Occupied
,
Free
,
Occupied
,
Free
,
Free
,
Occupied
},
{
Occupied
,
Free
,
Occupied
,
Free
,
Occupied
,
Occupied
},
{
Occupied
,
Free
,
Free
,
Free
,
Free
,
Occupied
},
...
...
@@ -28,13 +28,13 @@ grid_type grid4 = {{Occupied, Occupied, Occupied, Occupied, Occupied, Occupied},
{
Occupied
,
Free
,
Free
,
Free
,
Occupied
,
Occupied
},
{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
}};
grid
_type
grid5
=
{{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
},
grid
grid5
=
{{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
},
{
Occupied
,
Free
,
Free
,
Free
,
Free
,
Occupied
},
{
Occupied
,
Free
,
Free
,
Free
,
Free
,
Occupied
},
{
Occupied
,
Free
,
Free
,
Free
,
Free
,
Occupied
},
{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
}};
grid
_type
grid6
=
{{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
},
grid
grid6
=
{{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
},
{
Occupied
,
Free
,
Free
,
Free
,
Free
,
Occupied
},
{
Occupied
,
Free
,
Free
,
Free
,
Free
,
Occupied
},
{
Occupied
,
Free
,
Free
,
Free
,
Free
,
Occupied
},
...
...
@@ -48,7 +48,7 @@ grid_type grid6 = {{Occupied, Occupied, Occupied, Occupied, Occupied, Occupied},
{
Occupied
,
Free
,
Free
,
Free
,
Free
,
Occupied
},
{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
}};
grid
_type
grid7
=
{{
Occupied
,
Occupied
,
Occupied
,
Unknown
,
Occupied
,
Occupied
,
Occupied
},
grid
grid7
=
{{
Occupied
,
Occupied
,
Occupied
,
Unknown
,
Occupied
,
Occupied
,
Occupied
},
{
Occupied
,
Free
,
Frontier
,
Unknown
,
Frontier
,
Free
,
Occupied
},
{
Occupied
,
Free
,
Frontier
,
Frontier
,
Frontier
,
Free
,
Occupied
},
{
Occupied
,
Free
,
Free
,
Free
,
Free
,
Free
,
Occupied
},
...
...
@@ -60,7 +60,7 @@ grid_type grid7 = {{Occupied, Occupied, Occupied, Unknown, Occupied, Occupied, O
{
Occupied
,
Free
,
Free
,
Free
,
Free
,
Free
,
Occupied
},
{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
}};
grid
_type
grid8
=
{{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
},
grid
grid8
=
{{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
},
{
Occupied
,
Free
,
Free
,
Free
,
Free
,
Occupied
},
{
Occupied
,
Frontier
,
Frontier
,
Free
,
Free
,
Occupied
},
{
Occupied
,
Unknown
,
Frontier
,
Free
,
Free
,
Occupied
},
...
...
@@ -75,7 +75,7 @@ grid_type grid8 = {{Occupied, Occupied, Occupied, Occupied, Occupied, Occupied},
{
Occupied
,
Free
,
Free
,
Free
,
Free
,
Occupied
},
{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
}};
vector
<
grid
_type
>
test_1
=
{{
vector
<
grid
>
test_1
=
{{
{
Unknown
,
Unknown
,
Unknown
,
Unknown
},
{
Unknown
,
Unknown
,
Unknown
,
Unknown
},
{
Unknown
,
Unknown
,
Unknown
,
Unknown
},
...
...
@@ -95,7 +95,7 @@ vector<grid_type> test_1 = {{
{
Occupied
,
Occupied
,
Free
,
Occupied
},
{
Occupied
,
Occupied
,
Occupied
,
Occupied
}}};
vector
<
grid
_type
>
test_2
=
{
vector
<
grid
>
test_2
=
{
{
{
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
},
{
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
},
...
...
@@ -132,7 +132,7 @@ vector<grid_type> test_2 = {
{
Occupied
,
Free
,
Free
,
Free
,
Occupied
},
{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
}},
};
vector
<
grid
_type
>
test_3
=
{
vector
<
grid
>
test_3
=
{
{
{
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
},
{
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
},
...
...
@@ -187,7 +187,7 @@ vector<grid_type> test_3 = {
{
Occupied
,
Free
,
Free
,
Free
,
Free
,
Occupied
},
{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
}},
};
vector
<
grid
_type
>
test_4
=
{
vector
<
grid
>
test_4
=
{
{
{
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
},
{
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
},
...
...
@@ -217,7 +217,7 @@ vector<grid_type> test_4 = {
},
};
vector
<
grid
_type
>
test_5
=
{
vector
<
grid
>
test_5
=
{
{{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
},
{
Occupied
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Occupied
},
{
Occupied
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Occupied
},
...
...
@@ -256,7 +256,7 @@ vector<grid_type> test_5 = {
{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
}},
};
vector
<
grid
_type
>
test_6
=
{
vector
<
grid
>
test_6
=
{
{
{
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
},
{
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
},
...
...
@@ -303,7 +303,7 @@ vector<grid_type> test_6 = {
},
};
vector
<
grid
_type
>
test_7
=
{
vector
<
grid
>
test_7
=
{
{{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
},
{
Occupied
,
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Occupied
},
{
Occupied
,
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Unknown
,
Occupied
},
...
...
@@ -333,7 +333,7 @@ vector<grid_type> test_7 = {
{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
}},
};
vector
<
vector
<
grid
_type
>>
tests
=
{
test_1
,
test_2
,
test_3
,
test_4
,
test_5
,
test_6
,
test_7
};
vector
<
vector
<
grid
>>
tests
=
{
test_1
,
test_2
,
test_3
,
test_4
,
test_5
,
test_6
,
test_7
};
#define RESET "\033[0m"
#define BLACK "\033[30m"
/* Black */
...
...
@@ -351,7 +351,7 @@ string colorize(string s, string color) {
}
int
main
(
int
argc
,
char
**
argv
)
{
vector
<
grid
_type
>
test
=
test_
5
;
vector
<
grid
>
test
=
test_
2
;
if
(
argc
==
2
)
{
char
*
a
=
argv
[
1
];
int
num
=
atoi
(
a
);
...
...
@@ -361,7 +361,7 @@ int main(int argc, char** argv) {
}
test
=
tests
[
num
-
1
];
}
grid
_type
grid
;
grid
grid
;
GVDIC
igvdc
;
cout
<<
"# INITIALIZATION DONE"
<<
endl
;
...
...
src/lib/GVD/utils.h
View file @
9e3ac14a
...
...
@@ -51,7 +51,7 @@ ostream& operator<<(ostream& out, const map<K, T>& m) {
return
out
;
}