Help With implementation of A*
2 次查看(过去 30 天)
显示 更早的评论
I am having some troubles to implement the A star search algoritm to solve 1 problem. Could anyone send me his implementation of A*? Or help me with mine. Thanks!
clc;
clear all;
fprintf('Starting...\n\n');
%% Declaration of variables
vi=[]; %Creating de Initial Vector
r=0;
vgoal = [0 1 2 ;3 4 5 ;6 7 8];
MaxC = 3;
MaxR = 3;
while (r~=1)
% Loop to ask for the components of the Matrix to be introduce
for aux1=1 : MaxR %For 1 to Max number of Rows
for aux2=1 : MaxC
vi(aux1,aux2)= input('Enter the number in the matrix (from 1 to 8). Type "0" for the Empty one :\n');
end
end
% Show the Start State of the Matrix
fprintf('\nStart State: \n');
for aux1=1 : MaxR
for aux2=1 : MaxC
fprintf('\t %d\t',vi(aux1,aux2));
end
fprintf('\n');
end
% If you type 0, the program will ask you again for all the data
r=input('Is that the Matrix you wanted to create? Type "1" for YES or "0" for NO :\n');
end
vf=vi;
%% A* Search
g=0;
res=0;
while (getH(vf,vgoal)~=0)
for aux3=1 : MaxR
for aux4=1 : MaxC
if (vf(aux3,aux4)==0)
g= g+1;
lf=0;
rf=0;
uf=0;
df=0;
if(aux4~=1)
lv=vf;
lv(aux3,aux4)= lv(aux3,aux4-1);
lv(aux3,aux4-1)=0;
lh = getH(lv,vgoal);
lf = getF(lh,g);
else
lf=1000;
end
if (aux4~=3)
rv=vf ;
rv(aux3,aux4)= rv(aux3,aux4+1);
rv(aux3,aux4+1)=0;
rf = getF(getH(rv,vgoal),g);
else
rf=1000;
end
if(aux3~=1)
uv=vf;
uv(aux3,aux4)= uv(aux3-1,aux4);
uv(aux3-1,aux4)=0;
uf = getF(getH(uv,vgoal),g);
else
df=1000;
end
if (aux3~=3)
dv=vf;
dv(aux3,aux4)= dv(aux3+1,aux4);
dv(aux3+1,aux4)=0;
df = getF(getH(dv,vgoal),g);
else
uf=1000;
end
if ((lf<=rf) && (lf<=uf) && (lf<=df) && (lf~=0))
fprintf('\nLeft: \n');
N_exp=[
vf=lv;
end
if ((rf<lf) && (rf<uf) && (rf<df) && (rf~=0))
fprintf('\nRight: \n');
vf=rv;
end
if ((uf<lf) && (uf<rf) && (uf<df) && (uf~=0))
fprintf('\nUp: \n');
vf=uv;
end
if ((df<lf) && (df<uf) && (df<rf) && (df~=0))
fprintf('\nDown: \n');
vf=dv;
end
for aux5=1 : MaxR
for aux6=1 : MaxC
fprintf('\t %d\t',vf(aux5,aux6));
end
fprintf('\n');
end
fprintf('\n');
end
end
end
end
%% Final Outputs
% Show the final State of the Matrix
fprintf('\nGoal State: \n');
for aux1=1 : MaxR
for aux2=1 : MaxC
fprintf('\t %d\t',vf(aux1,aux2));
end
fprintf('\n');
end
%% Functions
function H = getH(vm,vgoal)
H=0;
MaxR=3;
MaxC=3;
for aux1=1 : MaxR
for aux2=1 : MaxC
if (vm(aux1,aux2)~=vgoal(aux1,aux2))
H = H+1;
end
end
end
end
function getF = getF(h_score,g_score)
getF=h_score+g_score;
end
1 个评论
Image Analyst
2021-10-15
Did you check the File Exchange? Also, it might help if you added a LOT more comments to your code.
回答(1 个)
arushi
2023-10-13
Hi Sergio,
I understand that you want to implement ‘A star’ algorithm.
Please find the code below that takes the following argument as input:
A logical 2d matrix that represents a map. The logic ‘TRUE’ specifies a visitable map cell, and ‘FALSE’ indicates that the cell is not reachable. The ‘A star’ algorithm finds the shortest path through the map. A ‘cost’ specified for each map cell, which penalizes visits to that cell.
function final = a_star(map, costs, start, goal)
if ~islogical(map)
error('MAP must be logical')
end
if ~isa(costs, 'double')
error('COSTS must be double')
end
% Avoid div by zero
costs(costs == 0) = eps;
% Normalize such that smallest cost is 1.
costs = costs / min(costs(:));
% default return - empty for failure case
final = [];
mapSize = size(map);
mapNumEl = numel(mapSize);
% Initialize the open set, with START
openSet = false(mapSize);
openSet(start) = true;
% Initialize closed set. Closed set consists of visited locations on
% the map
closedSet = false(mapSize);
cameFrom = zeros(1, mapNumEl);
gScore = inf(mapSize);
gScore(start) = 0;
% Linear index -> row, col subscripts for the goal
[gr, gc] = ind2sub(mapSize, goal);
fScore = inf(mapSize);
fScore(start) = compute_cost(mapSize, start, gr, gc);
S2 = sqrt(2);
% While the open set is not empty
while any(openSet(:) > 0)
% Find the minimum fScore within the open set
[~, current] = min(fScore(:));
% If we've reached the goal
if current == goal
% Get the full path and return it
final = get_path(cameFrom, current);
return
end
% Linear index -> row, col subscripts
rc = rem(current - 1, mapSize(1)) + 1;
cc = (current - rc) / mapSize(1) + 1;
% Remove CURRENT from openSet
openSet(rc, cc) = false;
% Place CURRENT in closedSet
closedSet(rc, cc) = true;
fScore(rc, cc) = inf;
gScoreCurrent = gScore(rc, cc) + costs(rc, cc);
% Get all neighbors of CURRENT. Neighbors are adjacent indices on
% the map, including diagonals.
% Col 1 = Row, Col 2 = Col, Col 3 = Distance to the neighbor
n_ss = [ ...
rc + 1, cc + 1, S2 ; ...
rc + 1, cc + 0, 1 ; ...
rc + 1, cc - 1, S2 ; ...
rc + 0, cc - 1, 1 ; ...
rc - 1, cc - 1, S2 ; ...
rc - 1, cc - 0, 1 ; ...
rc - 1, cc + 1, S2 ; ...
rc - 0, cc + 1, 1 ; ...
];
% keep valid indices only
valid_row = n_ss(:,1) >= 1 & n_ss(:,1) <= mapSize(1);
valid_col = n_ss(:,2) >= 1 & n_ss(:,2) <= mapSize(2);
n_ss = n_ss(valid_row & valid_col, :);
% subscripts -> linear indices
neighbors = n_ss(:,1) + (n_ss(:,2) - 1) .* mapSize(1);
% only keep neighbors in the map and not in the closed set
ixInMap = map(neighbors) & ~closedSet(neighbors);
neighbors = neighbors(ixInMap);
% distance to each kept neighbor
dists = n_ss(ixInMap, 3);
% Add each neighbor to the open set
openSet(neighbors) = true;
% TENTATIVE_GSCORE is the score from START to NEIGHBOR.
tentative_gscores = gScoreCurrent + costs(neighbors) .* dists;
% IXBETTER indicates where a better path was found
ixBetter = tentative_gscores < gScore(neighbors);
bestNeighbors = neighbors(ixBetter);
% For the better paths, update scores
cameFrom(bestNeighbors) = current;
gScore(bestNeighbors) = tentative_gscores(ixBetter);
fScore(bestNeighbors) = gScore(bestNeighbors) + compute_cost(mapSize, bestNeighbors, gr, gc);
end % while
end
function p = get_path(cameFrom, current)
% Returns the path. This function is only called once and therefore
% does not need to be extraordinarily efficient
inds = find(cameFrom);
p = nan(1, length(inds));
p(1) = current;
next = 1;
while any(current == inds)
current = cameFrom(current);
next = next + 1;
p(next) = current;
end
p(isnan(p)) = [];
end
function cost = compute_cost(mapSize, from, rTo, cTo)
% Returns COST, an estimated cost to travel the map, starting FROM and
% ending at TO.
[rFrom,cFrom] = ind2sub(mapSize, from);
cost = sqrt((rFrom - rTo).^2 + (cFrom - cTo).^2);
end
Hope this answers your question.
0 个评论
另请参阅
产品
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!