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
8ba93364
Commit
8ba93364
authored
Oct 20, 2020
by
Federico Ciuffardi
Browse files
some bug fixed and better integration with crit dist map
parent
4489169d
Changes
8
Hide whitespace changes
Inline
Side-by-side
src/lib/GVD/DisMapIC.cpp
View file @
8ba93364
#include "DisMapIC.h"
DisMapIC
::
DisMapIC
(
Map
*
m
,
vector
<
cell_type
>
obstructed_types
,
cell_type
source_type
){
DisMapIC
::
DisMapIC
(
Map
*
m
,
vector
<
cell_type
>
obstructed_types
){
this
->
m
=
m
;
this
->
obstructed_types
=
obstructed_types
;
this
->
source_type
=
source_type
;
};
// CellData
...
...
@@ -65,13 +64,13 @@ void DisMapIC::process_lower(pos p) {
n_cd
.
dist
=
d
;
// cout<<n<<"updated dist to "<< n_cd.dist <<endl;
n_cd
.
parents
.
clear
();
n_cd
.
parents
.
insert
(
n
);
n_cd
.
parents
.
insert
(
p
);
n_cd
.
sources
.
clear
();
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
.
parents
.
insert
(
p
);
n_cd
.
sources
.
insert
(
p_cd
.
sources
.
begin
(),
p_cd
.
sources
.
end
());
}
}
...
...
@@ -92,12 +91,13 @@ void DisMapIC::process_raise(pos p) {
if
(
!
n_cd
.
is_cleared
&&
!
n_cd
.
to_raise
)
{
// if n is not valid it should propagate the process raise
n_cd
.
parents
.
erase
(
p
);
//cout<<n<<" parents "<<n_cd.parents<<endl;
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
{
}
else
if
(
!
is_elem
(
obstructed_types
,(
*
m
)[
p
])
)
{
// if is valid then it could be used
// to obtain the disstance of p
float
d
=
dist
(
p
,
n
)
+
n_cd
.
dist
;
...
...
@@ -134,7 +134,7 @@ void DisMapIC::set_consistent_borders(vector<pos> obstructed_to_free) {
pos_set
inserted
;
for
(
int
i
=
0
;
i
<
obstructed_to_free
.
size
();
i
++
)
{
pos
p
=
obstructed_to_free
[
i
];
// cout << "
unk
_to_free"<<p << endl;
// cout << "
obstructed
_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
);
...
...
@@ -146,8 +146,8 @@ void DisMapIC::set_consistent_borders(vector<pos> obstructed_to_free) {
}
}
}
std
::
cout
<<
"consistent borders"
<<
endl
;
print_property
(
inserted
,
m
->
g
);
//
std::cout << "consistent borders" << endl;
//
print_property(inserted, m->g);
}
void
DisMapIC
::
set_source
(
pos
p
)
{
...
...
@@ -175,29 +175,45 @@ void DisMapIC::obstruct(pos p) {
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
,
vector
<
pos
>
free_to_obstructed
){
// cout<<endl<<"# Getting changes"<<endl;
void
DisMapIC
::
update_distance_map
(
pos_set
source_to_true
,
pos_set
source_to_false
){
vector
<
pos
>
obstructed_to_free
,
free_to_obstructed
;
for
(
auto
it
=
m
->
changes
.
begin
();
it
!=
m
->
changes
.
end
();
it
++
){
pos
p
=
it
->
first
;
// source status changes and obstruction status change are mutally exclusive
// and source status changes have priority
if
(
is_elem
(
source_to_false
,
p
)
||
is_elem
(
source_to_true
,
p
)){
continue
;
}
cell_type
old_ct
=
it
->
second
.
first
;
cell_type
new_ct
=
it
->
second
.
second
;
if
(
is_elem
(
obstructed_types
,
old_ct
)
&&
!
is_elem
(
obstructed_types
,
new_ct
)){
obstructed_to_free
.
push_back
(
p
);
}
else
if
(
!
is_elem
(
obstructed_types
,
old_ct
)
&&
is_elem
(
obstructed_types
,
new_ct
)){
free_to_obstructed
.
push_back
(
p
);
}
}
//cout<<endl<<"# CRIT"<<endl;
// cout<<endl<<"# setting cd for the changes"<<endl;
for
(
int
i
=
0
;
i
<
source_to_false
.
size
();
i
++
)
{
remove_source
(
source_to_false
[
i
]);
// cout << "Remove an source source" << obs_to_free[i] << endl;
for
(
auto
it
=
source_to_false
.
begin
();
it
!=
source_to_false
.
end
();
it
++
)
{
pos
p
=
*
it
;
remove_source
(
p
);
//cout << "remove_source " << p << endl;
}
for
(
int
i
=
0
;
i
<
free_to_obstructed
.
size
();
i
++
)
{
obstruct
(
free_to_obstructed
[
i
]);
//
cout << "
Set an source
" <<
any
_to_obs[i] << endl;
//cout << "
obstruct
" <<
free
_to_obs
tructed
[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;
for
(
auto
it
=
source_to_true
.
begin
();
it
!=
source_to_false
.
end
();
it
++
)
{
pos
p
=
*
it
;
set_source
(
p
);
//cout << "set_source " << p << endl;
}
// cout<<endl<<"# Rebuild GVD"<<endl;
...
...
@@ -213,10 +229,10 @@ void DisMapIC::raise_and_lower(){
CellData
&
p_cd
=
cell_data
[
p
];
if
(
p_cd
.
to_raise
)
{
//
cout << "Process RAISE "<<p << endl;
//cout << "Process RAISE "<<p << endl;
process_raise
(
p
);
}
else
if
(
valid
(
p_cd
))
{
//
cout << "Process LOWER "<<p << endl;
//cout << "Process LOWER "<<p << endl;
process_lower
(
p
);
}
}
...
...
src/lib/GVD/DisMapIC.h
View file @
8ba93364
...
...
@@ -41,13 +41,8 @@ class DisMapIC{
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
;
Map
*
m
=
NULL
;
...
...
@@ -60,5 +55,6 @@ class DisMapIC{
pos_set
voro_to_false
;
pos_set
voro_to_true
;
DisMapIC
(
Map
*
m
,
vector
<
cell_type
>
obstructed_types
,
cell_type
source_type
);
void
update_distance_map
(
pos_set
source_to_true
,
pos_set
source_to_false
);
DisMapIC
(
Map
*
m
,
vector
<
cell_type
>
obstructed_types
);
};
src/lib/GVD/GVDIC.cpp
View file @
8ba93364
...
...
@@ -4,6 +4,12 @@
#include "pos.h"
#include "utils.h"
// TODO move these as IGVDIC attributes
pos_set
obst_cond_to_false
;
// contanis all the cells that had 2 or more obstacles but now have 1 or less
pos_set
updated_obst_size
;
// containse all the cells that had a change on the obstacle size
// pos_set region_border; // contains all the new region borders
pos_set
old_region_border
;
// cointais all the cell that were region borders but one of the obstacles was removed
//////////
// AUX
//////////
...
...
@@ -26,11 +32,10 @@ boost::tuple<float, vector<pos>> closest_obstacles(pos p, pos_set obsts) {
return
boost
::
make_tuple
(
d
,
closest_obsts
);
}
pos_set
obst_cond_to_false
;
// contanis all the cells that had 2 or more obstacles but now have 1 or less
pos_set
updated_obst_size
;
// wrapers to set (or replace) and to insert (or union) obstacles that checks if obst_cond_to_false must be modified
// functions to set (or replace) and to insert (or union) obstacles that checks if obst_cond_to_false must be modified
// the funcionality provided by the obst_cont_to_false may be already provided by the old_region_border should be
// reviewed for efficiency reasons
// must be called after a obsts update
void
obsts_updated_hook
(
pos
p
,
int
old_size
,
int
pre_update_size
,
int
updated_size
,
bool
is_obst
=
false
){
...
...
@@ -120,6 +125,7 @@ void GVDIC::set_voro(bool value, pos n) {
CellDataO
&
n_cd
=
cell_data_o
[
n
];
// cout<<"Celda: "<<n<<" valor: "<<value<<endl;
if
(
value
)
{
// region_border.insert(n);
if
(
!
is_elem
(
gvd
.
full_graph
,
n
)){
voro_to_true
.
insert
(
n
);
}
else
{
...
...
@@ -132,6 +138,7 @@ void GVDIC::set_voro(bool value, pos n) {
voro_to_false
.
erase
(
n
);
}
}
else
{
// region_border.erase(n);
if
(
is_elem
(
gvd
.
full_graph
,
n
)){
voro_to_false
.
insert
(
n
);
distance_update
.
erase
(
n
);
...
...
@@ -224,6 +231,8 @@ void GVDIC::process_raise(pos s) {
n_cd
.
clear_cell
();
n_cd
.
to_raise
=
true
;
}
else
{
if
(
is_elem
(
gvd
.
full_graph
,
s
))
old_region_border
.
insert
(
s
);
if
(
is_elem
(
gvd
.
full_graph
,
n
))
old_region_border
.
insert
(
n
);
// if n has a valid obstacle then it could be used
// to obtain the disstance of s
float
d
;
...
...
@@ -454,11 +463,11 @@ int GVDIC::A(pos p) {
return
res
;
}
boost
::
tuple
<
pos_set
,
pos_set
>
GVDIC
::
erosion
(
pos_set
voro_to_true
)
{
boost
::
tuple
<
pos_set
,
pos_set
>
GVDIC
::
erosion
(
pos_set
erosion_input
)
{
pos_set
to_add
,
to_erase
,
inserted
;
CellDataPQ
erosion_queue
;
for
(
auto
it
=
voro_to_true
.
begin
();
it
!=
voro_to_true
.
end
();
it
++
)
{
for
(
auto
it
=
erosion_input
.
begin
();
it
!=
erosion_input
.
end
();
it
++
)
{
pos
p
=
*
it
;
if
(
!
is_elem
(
inserted
,
p
))
{
inserted
.
insert
(
p
);
...
...
@@ -475,8 +484,8 @@ boost::tuple<pos_set, pos_set> GVDIC::erosion(pos_set voro_to_true) {
}
}
}
/
*
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
();
...
...
@@ -518,18 +527,42 @@ pos_set GVDIC::get_to_update_min(pos_set to_change){
void
GVDIC
::
update_gvd
()
{
pos_set
to_add_gvd
,
to_erase_gvd
,
to_update_gvd
,
to_update_min
,
erosion_input
;
/* cout<<"voro_to_true"<<endl; */
/* print_property(voro_to_true,gt); */
/* 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
);
// 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
for
(
auto
it
=
old_region_border
.
begin
();
it
!=
old_region_border
.
end
();
it
++
){
pos
p
=
*
it
;
if
(
cell_data_o
[
p
].
is_voro
){
erosion_input
.
insert
(
p
);
}
}
//for(auto it = voro_to_false.begin(); it != voro_to_false.end(); it++){
// pos p = *it;
// pos_set adj_p = adj(gt, p);
// for(auto it_n = adj_p.begin(); it_n != adj_p.end(); it_n++){
// pos n = *it_n;
// if(cell_data_o[n].is_voro){
// erosion_input.insert(n);
// }
// }
// }
// cout<<"voro_to_true"<<endl;
// print_property(voro_to_true,gt);
// cout<<"old_region_border"<<endl;
// print_property(old_region_border,gt);
//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);
// erosion
boost
::
tie
(
to_add_gvd
,
to_erase_gvd
)
=
erosion
(
erosion_input
);
to_erase_gvd
.
insert
(
voro_to_false
.
begin
(),
voro_to_false
.
end
());
...
...
@@ -546,7 +579,7 @@ void GVDIC::update_gvd() {
gvd
.
is_local_min
[
p
]
=
is_local_min
(
p
);
}
// erase
v
// erase
from gvd
for
(
auto
it
=
to_erase_gvd
.
begin
();
it
!=
to_erase_gvd
.
end
();
it
++
)
{
gvd
.
erase_v
(
*
it
);
}
...
...
@@ -554,7 +587,7 @@ void GVDIC::update_gvd() {
// add set of v to gvd
gvd
.
add_v
(
to_add_gvd
);
// update
// update
the local mins on the gvd (uncollapsing and collapsing if pertinent)
to_update_gvd
=
to_update_min
;
for
(
auto
it
=
to_update_gvd
.
begin
();
it
!=
to_update_gvd
.
end
();
it
++
){
pos
p
=
*
it
;
...
...
@@ -634,8 +667,8 @@ void GVDIC::update_possible_crit(pos_set to_add_gvd, pos_set to_erase_gvd) {
///////
// pcrit distance map
///
void
update_possible_crit_distance_map
(){
void
GVDIC
::
update_possible_crit_distance_map
(){
disMapIC
->
update_distance_map
(
possible_crit_to_true
,
possible_crit_to_false
);
}
//////////
...
...
@@ -644,6 +677,10 @@ void update_possible_crit_distance_map(){
boost
::
tuple
<
criticals_info
,
GVD
>
GVDIC
::
get_points_of_interest
(
grid
gt
)
{
// pre increment
// region_border.clear();
old_region_border
.
clear
();
voro_to_true
.
clear
();
voro_to_false
.
clear
();
distance_update
.
clear
();
...
...
@@ -657,6 +694,9 @@ boost::tuple<criticals_info, GVD> GVDIC::get_points_of_interest(grid gt) {
obst_cond_to_false
.
clear
();
// increment
this
->
gt
=
gt
;
m
->
update
(
gt
);
vector
<
pos
>
obs_to_free
,
any_to_obs
,
unk_to_free
,
unk_to_notunk
;
boost
::
tie
(
obs_to_free
,
any_to_obs
,
unk_to_free
,
unk_to_notunk
)
=
get_changes
(
gt
);
...
...
@@ -678,3 +718,13 @@ boost::tuple<criticals_info, GVD> GVDIC::get_points_of_interest(grid gt) {
return
boost
::
make_tuple
(
criticals_info
(),
GVD
());
}
GVDIC
::
GVDIC
(){
m
=
new
Map
();
disMapIC
=
new
DisMapIC
(
m
,{
Occupied
,
Unknown
});
}
GVDIC
::~
GVDIC
(){
delete
m
;
delete
disMapIC
;
}
\ No newline at end of file
src/lib/GVD/GVDIC.h
View file @
8ba93364
...
...
@@ -31,8 +31,6 @@ class GVDIC {
void
clear_cell
();
};
grid
old_gt
;
grid
gt
;
pos_set
frontiers
;
boost
::
unordered_map
<
pos
,
int
>
know_neighbors
;
...
...
@@ -55,6 +53,7 @@ class GVDIC {
void
remove_obstacle
(
pos
p
);
void
set_consistent_borders
(
vector
<
pos
>
);
void
update_frontiers
(
vector
<
pos
>
);
void
update_possible_crit_distance_map
();
void
set_voro
(
bool
value
,
pos
n
);
void
process_lower
(
pos
p
);
...
...
@@ -77,7 +76,19 @@ class GVDIC {
bool
has_neighbour_of_deg
(
pos
p
,
int
deg
);
public:
// old_gt/gt are equivallent to Map, should replace old_gt and gt with map eventually
// GVDIC uses old_gt/gt (should use map) and DisMapIC is already using map
grid
old_gt
;
grid
gt
;
Map
*
m
;
DisMapIC
*
disMapIC
;
GVDIC
();
~
GVDIC
();
int
DEBUG
=
0
;
// should be private
...
...
src/lib/GVD/Map.cpp
View file @
8ba93364
...
...
@@ -5,7 +5,7 @@ cell_type& Map::operator[](pos p){
}
void
Map
::
update
(
grid
g
)
{
this
->
g
=
g
;
this
->
changes
.
clear
()
;
int
mapWidth
=
g
.
size
();
if
(
mapWidth
==
0
)
{
cout
<<
"PLEASE USE A NON EMPTY MAP"
<<
endl
;
...
...
@@ -13,9 +13,15 @@ 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
]));
cell_type
old_type
=
first
?
Unknown
:
this
->
g
[
x
][
y
];
cell_type
new_type
=
g
[
x
][
y
];
if
(
old_type
!=
new_type
){
this
->
changes
[
pos
(
x
,
y
)]
=
make_pair
(
old_type
,
new_type
);
}
}
}
this
->
g
=
g
;
first
=
false
;
}
// returns all *known* neighbors of a given pos p in the grid g
...
...
src/lib/GVD/Map.h
View file @
8ba93364
...
...
@@ -2,12 +2,15 @@
#include <vector>
#include "grid.h"
#include "boost/unordered_map.hpp"
struct
Map
{
bool
first
=
true
;
grid
g
;
set
<
pair
<
pos
,
cell_type
>>
changes
;
boost
::
unordered_map
<
pos
,
pair
<
cell_type
,
cell_type
>>
changes
;
cell_type
&
operator
[](
pos
p
);
...
...
src/lib/GVD/test.cpp
View file @
8ba93364
...
...
@@ -352,15 +352,6 @@ vector<grid> test_8 = {
{
Free
,
Free
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
},
},
{
{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
},
{
Free
,
Free
,
Free
,
Free
,
Occupied
,
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
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Free
,
Free
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
},
},
{
{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
},
{
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
},
...
...
@@ -372,8 +363,102 @@ vector<grid> test_8 = {
},
};
vector
<
grid
>
test_9
=
{
{
{
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
},
},
{
{
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
},
},
{
{
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
,
Occupied
,
Occupied
,
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
},
},
{
{
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
},
},
{
{
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
,
Occupied
},
{
Occupied
,
Occupied
,
Free
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Occupied
,
Occupied
,
Free
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Occupied
,
Occupied
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
,
Free
},
{
Occupied
,
Occupied
,
Free
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
Occupied
,
Occupied
,
Free
,
Free
,
Free
,
Occupied
,
Free
,
Free
,
Free
,
Free
},
{
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
},
},
};
vector
<
vector
<
grid
>>
tests
=
{
test_1
,
test_2
,
test_3
,
test_4
,
test_5
,
test_6
,
test_7
,
test_8
};
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
},
},
{
{
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
},
},
{
{
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
},
},
{
{
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
},
},
};
vector
<
vector
<
grid
>>
tests
=
{
test_1
,
test_2
,
test_3
,
test_4
,
test_5
,
test_6
,
test_7
,
test_8
,
test_9
,
test_10
};
#define RESET "\033[0m"
#define BLACK "\033[30m"
/* Black */
...
...
@@ -411,58 +496,62 @@ int main(int argc, char** argv) {
cout
<<
endl
<<
"# get_points_of_interest "
<<
t
<<
endl
;
igvdc
.
get_points_of_interest
(
test
[
t
]);
// cout<<"DIST_NEW: "<< igvdc.cell_data_o[pos(0,0)].dist << endl;
cout
<<
endl
<<
"# distance map "
<<
t
<<
endl
;
for
(
int
i
=
0
;
i
<
test
[
t
].
size
();
i
++
)
{
for
(
int
j
=
0
;
j
<
test
[
t
][
i
].
size
();
j
++
)
{
if
(
igvdc
.
cell_data_o
[
pos
(
i
,
j
)].
dist
==
MAXFLOAT
)
{
cout
<<
"| ∞"
;
}
else
{
cout
<<
"|"
<<
std
::
setfill
(
' '
)
<<
std
::
setw
(
7
)
<<
igvdc
.
cell_data_o
[
pos
(
i
,
j
)].
dist
;
}
}
cout
<<
"|"
<<
endl
;
}
cout
<<
endl
;
cout
<<
"IS_VORO"
<<
endl
;
for
(
int
i
=
0
;
i
<
test
[
t
].
size
();
i
++
)
{
for
(
int
j
=
0
;
j
<
test
[
t
][
i
].
size
();
j
++
)
{
pos
p
=
pos
(
i
,
j
);
if
(
igvdc
.
cell_data_o
[
p
].
dist
==
MAXFLOAT
)
{
cout
<<
"|"
<<
colorize
(
" ? "
,
BLACK
);
}
else
if
(
test
[
t
][
i
][
j
]
==
Occupied
)
{
cout
<<
"|"
<<
colorize
(
" "
,
BLACKFG
);
}
else
if
(
igvdc
.
cell_data_o
[
pos
(
i
,
j
)].
is_voro
)
{
cout
<<
"|"
<<
colorize
(
" O "
,
CYAN
);
}
else
{
cout
<<
"|"
<<
colorize
(
" "
,
WHITE
);
}
}
cout
<<
"|"
<<
endl
;
}
cout
<<
endl
;
cout
<<
"BOOST"
<<
endl
;
auto
gvd_positions
=
igvdc
.
gvd
.
positions
;
for
(
int
i
=
0
;
i
<
test
[
t
].
size
();
i
++
)
{
for
(
int
j
=
0
;
j
<
test
[
t
][
i
].
size
();
j
++
)
{
pos
p
=
pos
(
i
,
j
);
if
(
igvdc
.
cell_data_o
[
p
].
dist
==
MAXFLOAT
)
{
cout
<<
"|"
<<
colorize
(
" ? "
,
BLACK
);
}
else
if
(
test
[
t
][
i
][
j
]
==
Occupied
)
{
cout
<<
"|"
<<
colorize
(
" "
,
BLACKFG
);
}
else
if
(
igvdc
.
gvd
.
is_uncollapsed
(
p
))
{
cout
<<
"|"
<<
colorize
(
" O "
,
CYAN
);
}
else
if
(
igvdc
.
gvd
.
is_collapsed
(
p
))
{
cout
<<
"|"
<<
colorize
(
" * "
,
CYAN
);
}
else
{
cout
<<
"|"
<<
colorize
(
" "
,
WHITE
);
}
}
cout
<<
"|"
<<
endl
;
}
cout
<<
endl
;
cout
<<
"------------------------------"
<<
endl
;
cout