-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathfollow_APF.m
54 lines (53 loc) · 1.41 KB
/
follow_APF.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
function [dx, dy] = follow_APF(x1, y1, dx, dy, APF)
length = size(APF,1);
adj_APF = 255.*ones(8,1); % array of adjacent APFs [topleft, top, topright, left, right, bot_left, bot, bot_right]
if x1 > 1
adj_APF(4) = APF(x1-1,y1);
if y1 > 1
adj_APF(1) = APF(x1-1,y1-1);
adj_APF(2) = APF(x1,y1-1);
end
if y1 < length
adj_APF(6) = APF(x1-1,y1+1);
adj_APF(7) = APF(x1,y1+1);
end
end
if x1 < length
adj_APF(5) = APF(x1+1,y1);
if y1 > 1
adj_APF(2) = APF(x1,y1-1);
adj_APF(3) = APF(x1+1,y1-1);
end
if y1 < length
adj_APF(7) = APF(x1,y1+1);
adj_APF(8) = APF(x1+1,y1+1);
end
end
ind = 0;
[min_APF,ind] = min(adj_APF);
if min_APF < APF(x1,y1)
if ind == 1
dx = -1; dy = -1;
end
if ind == 2
dx = 0; dy = -1;
end
if ind == 3
dx = +1; dy = -1;
end
if ind == 4
dx = -1; dy = 0;
end
if ind == 5
dx = 1; dy = 0;
end
if ind == 6
dx = -1; dy = 1;
end
if ind == 7
dx = 0; dy = 1;
end
if ind == 8
dx = 1; dy = 1;
end
end