Skip to content

Commit e88e152

Browse files
author
Marco P. Nogueira
committed
Merge remote-tracking branch 'upstream/master'
2 parents 7a45ab4 + 5c4f425 commit e88e152

File tree

6 files changed

+383
-0
lines changed

6 files changed

+383
-0
lines changed

‎matlab_simulation/02-examples_practice/maze_sampling/PRM_maze_solver.m‎

Lines changed: 82 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,82 @@
1+
% Deterministic and Random Sampling for maze solving
2+
close all; clc; clear;
3+
4+
% Compute time check (Waslander Lenovo W530 = 0.39-0.46 s)
5+
tic;
6+
disp('Speed test: 0.39s on Lenovo W530. This machine:');
7+
inv(rand(2000));
8+
toc;
9+
10+
% Each row of the map structure draws a single line of the maze
11+
% with coordinates [x1 x2 y1 y2];
12+
% Top left corner of maze is [0.5 0.5],
13+
% Bottom right corner is [col+0.5
14+
% row+0.5]
15+
row = 15;
16+
col = 15;
17+
xMin = 0.5;
18+
yMin = 0.5;
19+
xMax = col+0.5;
20+
yMax = row+0.5;
21+
map = maze(row,col,'C');
22+
start = [0.5, 1.0];
23+
finish = [col+0.5, row];
24+
25+
% Sample points
26+
figure(1); hold on;
27+
plot(start(1), start(2), 'gx')
28+
plot(finish(1), finish(2), 'rx')
29+
30+
% Create Milestones
31+
tic;
32+
nS = 2500;
33+
milestones = [row*rand(nS,1)+xMin col*rand(nS,1)+xMin];
34+
milestones(nS+1,:) = start;
35+
milestones(nS+2,:) = finish;
36+
nM = nS+2;
37+
disp('Time to create milestones');
38+
toc;
39+
plot(milestones(:,1),milestones(:,2),'m.');
40+
drawnow;
41+
42+
tic;
43+
p = 20;
44+
e = zeros(nM,nM);
45+
D = zeros*ones(nM,nM);
46+
for i = 1:nM
47+
% Find closest neighbours
48+
for j = 1:nM
49+
d(j) = norm(milestones(i,:)-milestones(j,:));
50+
end
51+
[d2,ind] = sort(d);
52+
% Check for edge collisions (no need to check if entire edge is
53+
% contained in obstacles as both endpoints are in free space)
54+
for j=1:p
55+
cur = ind(j);
56+
if (i<cur)
57+
if (~CheckCollisionMaze(milestones(i,:),milestones(cur,:), map))
58+
e(i,cur) = 1;
59+
e(cur,i) = 1;
60+
plot([milestones(i,1) milestones(cur,1)],[milestones(i,2) milestones(cur,2)],'m');
61+
end
62+
end
63+
end
64+
end
65+
disp('Time to find edges');
66+
toc;
67+
drawnow;
68+
69+
% Find shortest path
70+
tic;
71+
[sp, sd] = shortestpath_mr(milestones, e, nM-1, nM, 1, 1, 0);
72+
if length(sp) == 0
73+
disp('Failed to find path')
74+
else
75+
for i=1:length(sp)-1
76+
plot(milestones(sp(i:i+1),1),milestones(sp(i:i+1),2), 'go-', 'LineWidth',3);
77+
end
78+
title('Solved Maze');
79+
disp('Time to find shortest path');
80+
end
81+
toc;
82+

‎matlab_simulation/02-examples_practice/maze_sampling/deterministic_maze_solver.m‎

Lines changed: 105 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,105 @@
1+
% Deterministic and Random Sampling for maze solving
2+
close all; clc; clear;
3+
4+
% Compute time check (Waslander Lenovo W530 = 0.39-0.46 s)
5+
tic;
6+
disp('Speed test: 0.39 on Lenovo W530. This machine:');
7+
inv(rand(2000));
8+
toc;
9+
10+
% Each row of the map structure draws a single line of the maze
11+
% with coordinates [x1 x2 y1 y2];
12+
% Top left corner of maze is [0.5 0.5],
13+
% Bottom right corner is [col+0.5
14+
% row+0.5]
15+
row = 100;
16+
col = 100;
17+
xMin = 0.5;
18+
yMin = 0.5;
19+
xMax = col+0.5;
20+
yMax = row+0.5;
21+
map = maze(row,col, 'C');
22+
start = [0.5, 1.0];
23+
finish = [col+0.5, row];
24+
25+
% Sample points
26+
[rr,cc]=meshgrid(1:col,1:row);
27+
figure(1); hold on;
28+
% plot(rr, cc, 'cx')
29+
plot(start(1), start(2), 'gx')
30+
plot(finish(1), finish(2), 'rx')
31+
32+
% Create Milestones
33+
tic;
34+
nS = row*col;
35+
xlocs = 1:col;
36+
ylocs = 1:row;
37+
milestones(1,:) = start;
38+
milestones(2:nS+1,:) = combvec(xlocs,ylocs)';
39+
milestones(nS+2,:) = finish;
40+
nM = nS+2;
41+
disp('Time to create milestones')
42+
plot(milestones(:,1),milestones(:,2),'m.');
43+
drawnow;
44+
45+
tic;
46+
e = zeros(nM,nM);
47+
D = zeros*ones(nM,nM);
48+
for i = 1:nM
49+
% closest: [right left up down]
50+
51+
right = i+1;
52+
if(mod((i-1),col) ~= 0 && right > 0 && right < nM)
53+
j = right;
54+
if (j > 0 && j < nM)
55+
if (~CheckCollisionSquare2(milestones(i,:),milestones(j,:), map))
56+
e(i,j) = 1;
57+
e(j,i) = 1;
58+
plot([milestones(i,1) milestones(j,1)],[milestones(i,2) milestones(j,2)],'m');
59+
end
60+
end
61+
end
62+
63+
up = i+col;
64+
if(i ~= 1 && up > 0 && up < nM)
65+
j = up;
66+
if (j > 0 && j < nM)
67+
if (~CheckCollisionSquare2(milestones(i,:),milestones(j,:), map))
68+
e(i,j) = 1;
69+
e(j,i) = 1;
70+
plot([milestones(i,1) milestones(j,1)],[milestones(i,2) milestones(j,2)],'m');
71+
end
72+
end
73+
end
74+
end
75+
i = nM-1;
76+
j = nM;
77+
if (~CheckCollisionSquare2(milestones(i,:),milestones(j,:), map))
78+
e(i,j) = 1;
79+
e(j,i) = 1;
80+
plot([milestones(i,1) milestones(j,1)],[milestones(i,2) milestones(j,2)],'m');
81+
end
82+
i = 1;
83+
j = 2;
84+
if (~CheckCollisionSquare2(milestones(i,:),milestones(j,:), map))
85+
e(i,j) = 1;
86+
e(j,i) = 1;
87+
plot([milestones(i,1) milestones(j,1)],[milestones(i,2) milestones(j,2)],'m');
88+
end
89+
disp('Time to find edges');
90+
toc;
91+
drawnow;
92+
93+
% Find shortest path
94+
tic;
95+
[sp, sd] = shortestpath_mr(milestones, e, 1, nM, 1, 1, 0);
96+
if length(sp) == 0
97+
disp('Failed to find path')
98+
else
99+
for i=1:length(sp)-1
100+
plot(milestones(sp(i:i+1),1),milestones(sp(i:i+1),2), 'go-', 'LineWidth',3);
101+
end
102+
title('Solved Maze');
103+
disp('Time to find shortest path');
104+
end
105+
toc;

‎matlab_simulation/02-examples_practice/maze_sampling/integer_PRM_maze_solver.m‎

Lines changed: 96 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,96 @@
1+
% Deterministic and Random Sampling for maze solving
2+
close all; clc; clear;
3+
4+
% Compute time check (Waslander Lenovo W530 = 0.39-0.46 s)
5+
tic;
6+
disp('Speed test: 0.39 on Lenovo W530. This machine:');
7+
inv(rand(2000));
8+
toc;
9+
10+
% Each row of the map structure draws a single line of the maze
11+
% with coordinates [x1 x2 y1 y2];
12+
% Top left corner of maze is [0.5 0.5],
13+
% Bottom right corner is [col+0.5
14+
% row+0.5]
15+
row = 30;
16+
col = 30;
17+
xMin = 0.5;
18+
yMin = 0.5;
19+
xMax = col+0.5;
20+
yMax = row+0.5;
21+
map = maze(row,col, 'C');
22+
start = [0.5, 1.0];
23+
finish = [col+0.5, row];
24+
25+
% Sample points
26+
[rr,cc]=meshgrid(1:col,1:row);
27+
figure(1); hold on;
28+
% plot(rr, cc, 'cx')
29+
plot(start(1), start(2), 'gx')
30+
plot(finish(1), finish(2), 'rx')
31+
32+
% Create Milestones
33+
nS = 12000;
34+
35+
36+
tic;
37+
samples = floor([row*rand(nS,1)+xMin col*rand(nS,1)+yMin]);
38+
keep = ones(size(samples,1),1);
39+
for i = 1:nS
40+
if (keep(i) && i ~= nS)
41+
for j = i+1:nS
42+
if(samples(i,1) == samples(j,1) && samples(i,2) == samples(j,2))
43+
keep(j) = 0;
44+
end
45+
end
46+
end
47+
end
48+
keepSamples = samples(keep==1,:);
49+
numKeepSamples = size(keepSamples,1);
50+
milestones(1:numKeepSamples,:) = keepSamples;
51+
milestones(numKeepSamples+1,:) = start;
52+
milestones(numKeepSamples+2,:) = finish;
53+
nM = numKeepSamples+2;
54+
disp('Time to create milestones')
55+
toc;
56+
plot(milestones(:,1),milestones(:,2),'m.');
57+
drawnow;
58+
59+
tic;
60+
p = 10;
61+
e = zeros(nM,nM);
62+
D = zeros*ones(nM,nM);
63+
for i = 1:nM
64+
% Find closest neighbours
65+
for j = 1:nM
66+
d(j) = norm(milestones(i,:)-milestones(j,:));
67+
end
68+
[d2,ind] = sort(d);
69+
% Check for edge collisions (no need to check if entire edge is
70+
% contained in obstacles as both endpoints are in free space)
71+
for j=1:p
72+
cur = ind(j);
73+
if (~CheckCollisionMaze(milestones(i,:),milestones(cur,:), map))
74+
e(i,cur) = 1;
75+
e(cur,i) = 1;
76+
plot([milestones(i,1) milestones(cur,1)],[milestones(i,2) milestones(cur,2)],'m');
77+
end
78+
end
79+
end
80+
disp('Time to find edges');
81+
toc;
82+
drawnow;
83+
84+
% Find shortest path
85+
tic;
86+
[sp, sd] = shortestpath_mr(milestones, e, nM-1, nM, 1, 1, 0);
87+
if length(sp) == 0
88+
disp('Failed to find path')
89+
else
90+
for i=1:length(sp)-1
91+
plot(milestones(sp(i:i+1),1),milestones(sp(i:i+1),2), 'go-', 'LineWidth',3);
92+
end
93+
title('Solved Maze');
94+
disp('Time to find shortest path');
95+
end
96+
toc;

‎matlab_simulation/09-utilities/geometry/CheckCollisionMaze.m‎

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
function [ collided ] = CheckCollisionMaze( s, f, map )
2+
% parametrize the lines
3+
step = f - s;
4+
5+
for i=1:size(map,1)
6+
e1 = [map(i,1) map(i,3)];
7+
e2 = [map(i,2) map(i,4)];
8+
estep = e2 - e1;
9+
10+
q_num = (s(2)-e1(2))*step(1) - (s(1)-e1(1))*step(2);
11+
q_den = estep(2)*step(1) - estep(1)*step(2);
12+
13+
if(q_den == 0)
14+
%disp('q_den zero');
15+
continue;
16+
end
17+
18+
q = q_num/q_den;
19+
20+
if(q < 0 || q > 1)
21+
%disp('q not collided');
22+
continue;
23+
end
24+
25+
t_num = (s(2)-e1(2))*estep(1) - (s(1)-e1(1))*estep(2);
26+
t_den = q_den;
27+
%t_num = e1(1) + q*estep(1) - s(1);
28+
t = t_num / t_den;
29+
30+
if(t > 0 && t < 1)
31+
%disp('t collided');
32+
collided = 1;
33+
return;
34+
end
35+
end
36+
37+
collided = 0;
38+
end
39+

‎matlab_simulation/09-utilities/geometry/CheckCollisionSquare.m‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
function [ collided ] = CheckCollisionSquare( s, f, map )
2+
v = 0;
3+
u = 0;
4+
if(s(1) == f(1))
5+
% Find edge going right
6+
v = 2;
7+
u = 1;
8+
else
9+
% Find edge going up
10+
v = 1;
11+
u = 2;
12+
end
13+
targetEdge = (s(v)+f(v))/2;
14+
15+
for i=1:size(map,1)
16+
e1 = [map(i,1) map(i,3)];
17+
e2 = [map(i,2) map(i,4)];
18+
19+
if(e1(v) == e2(v) && e1(v) == targetEdge)
20+
if((e1(u) > s(u) && e2(u) < f(u)) || ...
21+
(e1(u) < s(u) && e2(u) > f(u)))
22+
collided = 1;
23+
return;
24+
end
25+
end
26+
end
27+
collided = 0;
28+
end
29+

‎matlab_simulation/09-utilities/geometry/CheckCollisionSquare2.m‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
function [ collided ] = CheckCollisionSquare2( s, f, map )
2+
v = 0;
3+
u = 0;
4+
if(s(1) == f(1))
5+
% Find edge going right
6+
v = 2;
7+
u = 1;
8+
else
9+
% Find edge going up
10+
v = 1;
11+
u = 2;
12+
end
13+
targetEdge = (s(v)+f(v))/2;
14+
15+
% Restrict edge intersection search to edges in target row/column that
16+
% are perpendicular to row/column
17+
a = (v-1)*2 + 1; % index 3 or 1
18+
edges = map((map(:,a)==targetEdge)&(map(:,a)==map(:,a+1)),:);
19+
20+
for i=1:size(edges,1)
21+
e1 = [edges(i,1) edges(i,3)];
22+
e2 = [edges(i,2) edges(i,4)];
23+
24+
if((e1(u) > s(u) && e2(u) < f(u)) || ...
25+
(e1(u) < s(u) && e2(u) > f(u)))
26+
collided = 1;
27+
return;
28+
end
29+
end
30+
collided = 0;
31+
end
32+

0 commit comments

Comments
 (0)