Help With implementation of A*

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

[Image Analyst]

Did you check the File Exchange? Also, it might help if you added a LOT more comments to your code.

Answer 1

[arushi]

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.