Finally Path Finding Works!

04.06.2015

Got my a-star working and in a separate function that will give the next step in the path. That way any pedestrian simulated can use it independently, and we can now introduce behaviour and decision making into the code. I still have to finish my real time programming assignment2 due on the 12^th. But before that, the working code. The comments should explain my steps but more detail will be in the report

function [ nxtstp,pathList ] = a_star5(Grid,startPos,goalPos  )

%Input need to be a matrix grid and start and finish pos

%   this will rund the a-star algorithm and return the next

%     step and the path list

%{ part of testing

%Grid=testmap(); %comes from input

%startPos=[2 1];

%goalPos =[6 3];

%}%

[gr gc]=size(Grid);

%contains 1=g,2=h,3=f,4=xParant,5=yParant

calcVal= zeros(gr,gc,5);

closedList(1,:)=[1 1];

closedList(1,:)=[];

openList(1,:) = startPos; %1st entry in open list is startPos

looptrue=1; %loop control

%do loop while openList is not empty and loopture=1

while(isempty(openList)==0 && looptrue==1)

[minf currentPos]=min_f(calcVal,openList);

%get neighbour nodes of currentPos

    i=-1;

    while (i<2 && looptrue==1)

        j=-1;

        while (j<2 && looptrue==1)

            %here we get for each surrouding neighbour

            if(i==0 && j==0)

                j=j+1;%dont eval current node

            end

            %set the node pos vector to n

            n=[currentPos(1)+i currentPos(2)+j];

            %set movement cost

            if (i==0 || j==0)

                moveCost=10;

            else

                moveCost=14;

            end

            %calculate g,h

            gn = abs(n(1)-startPos(1))+ abs(n(2)-startPos(2));

            hn = abs(goalPos(1)-n(1))+ abs(goalPos(2)-n(2));

            % now to check if n is a walkable

            if (isnWalkable(Grid,n))

                if (all(n==goalPos))%if n is the goal

                    %set parant information

                    calcVal(goalPos(1),goalPos(2),4)= currentPos(1);

                    calcVal(goalPos(1),goalPos(2),5)= currentPos(2);

                    looptrue=0;

                end

                %if n is in the closed list

                if(checkInList(closedList,n))

                else

                    %if n is in the openlist

                    if(checkInList(openList,n))

                        %if g of n is less than g+m of current

                        if(calcVal(n(1),n(2),1)> (calcVal(currentPos(1),currentPos(2),1)+moveCost))

                            %set the parant information

                            calcVal(n(1),n(2),4)=currentPos(1);

                            calcVal(n(1),n(2),5)=currentPos(2);

                        end

                    else

                        %not in the openLIst calc vals

                        calcVal(n(1),n(2),1)=calcVal(currentPos(1),currentPos(2),1)+moveCost;

                        calcVal(n(1),n(2),2)=hn;

                        calcVal(n(1),n(2),3)=calcVal(n(1),n(2),1)+ calcVal(n(1),n(2),2);

                        calcVal(n(1),n(2),4)=currentPos(1);

                        calcVal(n(1),n(2),5)=currentPos(2);

                        %now add to openLIst

                        openList(end+1,:)=n;

                       

                    end

                end

                %disp('walk')

            else

                %disp('no walk')

            end

           

            j=j+1;

        end

        i=i+1;

    end%end of checking all neighbours

   

    [u row]=checkInList(openList,currentPos);

    openList(row,:)=[];%del that vector from list

    closedList(end+1,:)=currentPos;

   

   

end %end do while loop

% now to reconstruct the path from goal

if(looptrue == 0)

    disp('path avil')

    pathList=reconstructPath(calcVal,goalPos,startPos);

else

    disp('no path')

end

nxtstp=pathList(end-1,:);

end