-
Notifications
You must be signed in to change notification settings - Fork 0
/
Astar_Synapse.m
126 lines (118 loc) · 4.49 KB
/
Astar_Synapse.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
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
function optimal_path = Astar_Synapse(BW, Source, R, Neurons, Goal, thetas, ...
fill_gap, theta_thre, connected)
%% Parameters
[MAX_Y, MAX_X] = size(BW);
Target = Neurons(connected > 0, :);
R_compare = 1.2 * R(connected > 0);
Reach_points = zeros(0, 2);
%% Put all background points on the Closed list
[close_y, close_x] = find(~BW);
CLOSED = [close_x, close_y];
CLOSED_COUNT = size(CLOSED, 1);
%% put starting nodes in OPEN list
% get start points with source neuron and radius
circle_area = circle_points(Neurons, round(R*1.3), BW);
start_points = circle_area{Source};
% path cost = 0
path_cost = 0;
% put starting nodes in OPEN list
OPEN_COUNT = size(start_points, 1);
OPEN = zeros(OPEN_COUNT, 8);
for i = 1 : OPEN_COUNT
xNode = start_points(i, 1);
yNode = start_points(i, 2);
% parent_index = 0 stands for start
parent_index = 0;
% distance
[goal_distance, target_index] = distance(xNode, yNode, xTarget, yTarget);
% theta
former_theta = thetas(yNode, xNode);
% insert
OPEN(i, :) = insert_open(xNode, yNode, parent_index, path_cost,...
goal_distance, goal_distance, target_index, former_theta);
end
%% Loop
NoPath = 1;
while true
%% Find out the node with the smallest fn
index_min_node = min_fn(OPEN, OPEN_COUNT, xTarget, yTarget);
if (index_min_node ~= -1)
%Set xNode and yNode to the node with minimum fn
xNode = OPEN(index_min_node, 2);
yNode = OPEN(index_min_node, 3);
path_cost = OPEN(index_min_node, 5);%Update the cost of reaching the parent node
%Move the Node to list CLOSED
parent_index = index_min_node;
CLOSED_COUNT = CLOSED_COUNT + 1;
CLOSED(CLOSED_COUNT, :) = [xNode, yNode];
OPEN(index_min_node, 1) = 0;
plot(xNode,yNode, 'g+');
else
%No path exists to the Target!!
break;
end%End of index_min_node check
%% whether reach other neurons
dis = mydistance([xNode, yNode], Target);
if dis < R_compare
Reach_points = [Reach_points; xNode, yNode];
else
exp_array = expand_array(xNode,yNode,path_cost,xTarget,yTarget,CLOSED,...
MAX_X,MAX_Y, former_theta, thetas, fill_gap, theta_thre);
end
exp_count = size(exp_array,1);
%UPDATE LIST OPEN WITH THE SUCCESSOR NODES
%OPEN LIST FORMAT
%--------------------------------------------------------------------------
%IS ON LIST 1/0 | X val | Y val | Parent index | |h(n) |g(n)|f(n)|
%--------------------------------------------------------------------------
%EXPANDED ARRAY FORMAT
%--------------------------------
%| X val | Y val | h(n) | g(n) | f(n) | theta
%--------------------------------
for i = 1 : exp_count
% whether expanded point in OPEN list
exp_point = exp_array(i, :);
[Isopen, Location] = ismember(exp_point(1:2), OPEN(:,2:3), 'rows');
if Isopen
if exp_point(5) < OPEN(Location, 7)
OPEN(Location, 4:7) = [OPEN_COUNT, exp_point(3:end)];
end
else
OPEN_COUNT = OPEN_COUNT + 1;
OPEN(OPEN_COUNT, :) = insert_open(exp_point(1), exp_point(2), ...
parent_index, exp_point(3), ...
exp_point(4), exp_point(5));
end%End of insert new element into the OPEN list
end%End of i for
end%End of While Loop
%Once algorithm has run The optimal path is generated by starting of at the
%last node(if it is the target node) and then identifying its parent node
%until it reaches the start node.This is the optimal path
if NoPath
optimal_path = [];
fprintf('Fail to find path');
else
% last point in CLOSED as terminal
terminal = CLOSED(end, :);
optimal_path = terminal;
path_len = 1;
node_index = ismember(OPEN(:, 2:3), terminal, 'rows');
% parent node of terminal
parent = OPEN(node_index, 4);
% path
while OPEN(parent, 2:3) ~= Start
% add current point
path_len = path_len + 1;
optimal_path(path_len, :) = OPEN(parent, 2:3);
% find parent of current point
parent = OPEN(parent, 4);
end
% %Plot the Optimal Path!
% p = plot(optimal_path(end, 1), optimal_path(end, 2), 'bo');
% for i = path_len - 1 : -1 : 1
% pause(.25);
% set(p, 'XData', optimal_path(i,1), 'YData', optimal_path(i,2));
% drawnow;
% end
% plot(optimal_path(:,1), optimal_path(:, 2));
end