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
63c35f91
Commit
63c35f91
authored
Oct 13, 2020
by
Federico Ciuffardi
Committed by
Romina Julieta Parada Venossa
Oct 17, 2020
Browse files
to test
parent
2fa22114
Changes
4
Hide whitespace changes
Inline
Side-by-side
src/lib/GVD/DisMapIC.cpp
View file @
63c35f91
...
...
@@ -19,51 +19,47 @@ void DisMapIC::CellData::clear_cell() {
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
DisMapIC
::
valid
(
pos_set
&
sources
)
{
void
DisMapIC
::
clean_invalid_sources
(
pos_set
&
sources
)
{
for
(
auto
it
=
sources
.
begin
();
it
!=
sources
.
end
();)
{
pos
source
=
(
*
it
);
if
(
(
*
m
)[
source
]
!=
source_type
)
{
if
(
cell_data
[
source
].
dist
!=
0
)
{
it
=
sources
.
erase
(
it
);
}
else
{
++
it
;
}
}
return
sources
.
size
()
>
0
;
}
void
DisMapIC
::
process_lower
(
pos
s
)
{
CellData
&
s_cd
=
cell_data
[
s
];
/* returns true only if the cd has a valid source and
has a parent thats also has a parent.
reason: not taking the next ring of a raise as valid */
bool
DisMapIC
::
valid
(
CellData
cd
){
clean_invalid_sources
(
cd
.
sources
);
if
(
cd
.
sources
.
empty
()
||
cd
.
parents
.
empty
()){
return
false
;
}
for
(
auto
it
=
cd
.
parents
.
begin
();
it
!=
cd
.
parents
.
end
();
it
++
){
pos
n
=
*
it
;
if
(
!
cell_data
[
n
].
parents
.
empty
()){
return
true
;
}
}
return
false
;
}
void
DisMapIC
::
process_lower
(
pos
p
)
{
CellData
&
p_cd
=
cell_data
[
p
];
pos_set
adj_
s
=
m
->
adj
(
s
,
obstructed_types
);
for
(
auto
it
=
adj_
s
.
begin
();
it
!=
adj_
s
.
end
();
it
++
)
{
pos_set
adj_
p
=
m
->
adj
(
p
,
obstructed_types
);
for
(
auto
it
=
adj_
p
.
begin
();
it
!=
adj_
p
.
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;
float
d
=
dist
(
n
,
p
)
+
p_cd
.
dist
;
if
(
d
<
n_cd
.
dist
)
{
// cout << "lowered: " <<n << endl;
n_cd
.
dist
=
d
;
...
...
@@ -71,87 +67,79 @@ void DisMapIC::process_lower(pos s) {
n_cd
.
parents
.
clear
();
n_cd
.
parents
.
insert
(
n
);
n_cd
.
sources
.
clear
();
n_cd
.
sources
.
insert
(
s
_c
losest_
sources
.
begin
(),
s
_c
losest_
sources
.
end
());
n_cd
.
sources
.
insert
(
p
_c
d
.
sources
.
begin
(),
p
_c
d
.
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
))
{
check_voro
(
s
,
n
);
// cout << "check_voro" << n << endl;
}
}
else
if
(
d
==
n_cd
.
dist
)
{
n_cd
.
parents
.
insert
(
n
);
n_cd
.
sources
.
insert
(
p_cd
.
sources
.
begin
(),
p_cd
.
sources
.
end
());
}
}
}
}
void
DisMapIC
::
process_raise
(
pos
s
)
{
CellData
&
s
_cd
=
cell_data
[
s
];
void
DisMapIC
::
process_raise
(
pos
p
)
{
CellData
&
p
_cd
=
cell_data
[
p
];
float
min_d
=
MAXFLOAT
;
pos_set
s
_closest_sources
;
vector
<
pos
>
s
_parents
;
// for each neighbor n of
s
pos_set
adj_
s
=
m
->
adj
(
s
,
obstructed_types
);
for
(
auto
it
=
adj_
s
.
begin
();
it
!=
adj_
s
.
end
();
it
++
)
{
pos_set
p
_closest_sources
;
vector
<
pos
>
p
_parents
;
// for each neighbor n of
p
pos_set
adj_
p
=
m
->
adj
(
p
,
obstructed_types
);
for
(
auto
it
=
adj_
p
.
begin
();
it
!=
adj_
p
.
end
();
it
++
)
{
pos
n
=
(
*
it
);
CellData
&
n_cd
=
cell_data
[
n
];
if
(
!
n_cd
.
is_cleared
&&
!
n_cd
.
to_raise
)
{
// if n
doe
s not
have any valid sourceacle then its distance is invalid
// it and should propagate the process
ra
i
se
if
(
!
valid
(
n_cd
.
sources
))
{
// if n
i
s not
valid it should propagate the process raise
n_cd
.
parents
.
e
rase
(
p
);
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
{
// 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
);
// if is valid then it could be used
// to obtain the disstance of p
float
d
=
dist
(
p
,
n
)
+
n_cd
.
dist
;
// 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
(
n
);
s
_closest_sources
.
clear
();
s
_closest_sources
.
insert
(
n_c
losest_
sources
.
begin
(),
n_c
losest_
sources
.
end
());
p
_parents
.
clear
();
p
_parents
.
push_back
(
n
);
p
_closest_sources
.
clear
();
p
_closest_sources
.
insert
(
n_c
d
.
sources
.
begin
(),
n_c
d
.
sources
.
end
());
}
else
if
(
d
==
min_d
)
{
s
_closest_sources
.
insert
(
n_c
losest_
sources
.
begin
(),
n_c
losest_
sources
.
end
());
s
_parents
.
push_back
(
n
);
p
_closest_sources
.
insert
(
n_c
d
.
sources
.
begin
(),
n_c
d
.
sources
.
end
());
p
_parents
.
push_back
(
n
);
}
}
}
}
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
;
open
.
push
(
make_pair
(
s
_cd
.
dist
,
s
));
p
_cd
.
dist
=
min_d
;
p
_cd
.
parents
.
clear
();
p
_cd
.
parents
.
insert
(
p
_closest_sources
.
begin
(),
p
_closest_sources
.
end
());
p
_cd
.
sources
.
clear
();
p
_cd
.
sources
.
insert
(
p
_closest_sources
.
begin
(),
p
_closest_sources
.
end
());
p
_cd
.
is_cleared
=
false
;
open
.
push
(
make_pair
(
p
_cd
.
dist
,
p
));
}
s
_cd
.
to_raise
=
false
;
p
_cd
.
to_raise
=
false
;
}
void
DisMapIC
::
set_consistent_borders
(
vector
<
pos
>
unk
_to_free
)
{
void
DisMapIC
::
set_consistent_borders
(
vector
<
pos
>
obstructed
_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
++
)
{
for
(
int
i
=
0
;
i
<
obstructed
_to_free
.
size
();
i
++
)
{
pos
p
=
obstructed
_to_free
[
i
];
// cout << "unk_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
);
CellData
&
n_cd
=
cell_data
[
n
];
if
(
valid
(
n_cd
.
sources
)
&&
!
is_elem
(
inserted
,
s
))
{
if
(
valid
(
n_cd
)
&&
!
is_elem
(
inserted
,
p
))
{
// cout << " border:"<<n << endl;
open
.
push
(
make_pair
(
n_cd
.
dist
,
n
));
inserted
.
insert
(
n
);
...
...
@@ -162,6 +150,31 @@ void DisMapIC::set_consistent_borders(vector<pos> unk_to_free) {
print_property
(
inserted
,
m
->
g
);
}
void
DisMapIC
::
set_source
(
pos
p
)
{
CellData
&
p_cd
=
cell_data
[
p
];
p_cd
.
dist
=
0
;
p_cd
.
parents
.
clear
();
p_cd
.
parents
.
insert
(
p
);
p_cd
.
sources
.
clear
();
p_cd
.
sources
.
insert
(
p
);
p_cd
.
is_cleared
=
false
;
open
.
push
(
make_pair
(
0
,
p
));
}
void
DisMapIC
::
remove_source
(
pos
p
)
{
CellData
&
p_cd
=
cell_data
[
p
];
p_cd
.
clear_cell
();
p_cd
.
to_raise
=
true
;
open
.
push
(
make_pair
(
0
,
p
));
}
void
DisMapIC
::
obstruct
(
pos
p
)
{
CellData
&
p_cd
=
cell_data
[
p
];
open
.
push
(
make_pair
(
p_cd
.
dist
,
p
));
p_cd
.
clear_cell
();
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
,
...
...
@@ -174,26 +187,37 @@ void DisMapIC::update_distance_map(vector<pos> source_to_true,
// cout << "Remove an source source" << obs_to_free[i] << endl;
}
set_consistent_borders
(
obstructed_to_free
);
for
(
int
i
=
0
;
i
<
free_to_obstructed
.
size
();
i
++
)
{
obstruct
(
free_to_obstructed
[
i
]);
// cout << "Set an source " << any_to_obs[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;
}
// cout<<endl<<"# Rebuild GVD"<<endl;
raise_and_lower
();
}
void
DisMapIC
::
raise_and_lower
(){
while
(
!
open
.
empty
())
{
pos
s
=
open
.
top
().
second
;
pos
p
=
open
.
top
().
second
;
open
.
pop
();
CellData
&
s
_cd
=
cell_data
[
s
];
CellData
&
p
_cd
=
cell_data
[
p
];
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
);
if
(
p
_cd
.
to_raise
)
{
// cout << "Process RAISE "<<
p
<< endl;
process_raise
(
p
);
}
else
if
(
valid
(
p
_cd
))
{
// cout << "Process LOWER "<<
p
<< endl;
process_lower
(
p
);
}
}
}
\ No newline at end of file
src/lib/GVD/DisMapIC.h
View file @
63c35f91
...
...
@@ -27,20 +27,25 @@ class DisMapIC{
CellDataPQ
open
;
// functions
bool
valid
(
pos_set
&
sources
);
bool
valid
(
CellData
cd
);
void
clean_invalid_sources
(
pos_set
&
sources
);
void
check_voro
(
pos
s
,
pos
n
);
void
set_source
(
pos
p
);
void
remove_source
(
pos
p
);
void
obstruct
(
pos
p
);
void
unobstruct
(
pos
p
);
void
set_consistent_borders
(
vector
<
pos
>
);
void
process_lower
(
pos
p
);
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
;
...
...
src/lib/GVD/Map.cpp
View file @
63c35f91
...
...
@@ -13,7 +13,7 @@ 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
]);
this
->
changes
.
insert
(
make_pair
(
pos
(
x
,
y
),
g
[
x
][
y
])
)
;
}
}
}
...
...
src/lib/GVD/test
View file @
63c35f91
No preview for this file type
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment