Path finding In Progress!

03.28.2015

Currently debugging my a –star,

function [ ] = a_starPath3()

%will return the next step in the path

%   Detailed explanation goes here

%inputs

Grid=testmap();

currentPos=[1 3];

startPos=currentPos;

destinPos=[3 5];

%multidim matrix to store the g,h,f, and previous cell value (i=4,j=5)values and keep it

[x,y]=size(Grid);

calcVal=zeros(x+1,y+1,5);

%add the current pos to the openList matrix

openList(1,:)=currentPos;% openList and fval are going to be paired so they will have the same indexes

fval=zeros(size(Grid)+1);

fval(currentPos(1),currentPos(2))=1;% set the f value of the current cell, offset by 1

calcVal(currentPos(1),currentPos(2),3)=0;% moving the f val

cameFrom=currentPos;

%add and remove row from closed list

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

closedList(1,:)=[];

%--------------------------------------------------------------

%start loop here - do while open list is not empty

stoploop=0;

while (~isempty(openList)&& stoploop==0)

%find lowest f in openList

lowestf= calcVal(openList(1,1),openList(1,2),3)%1st element of openList will be set as the initial sloweset

fk=1;

for k=1:size(openList,1)

    if (~(lowestf==0))%if its zero, its the start

        if (calcVal(openList(k,1),openList(k,2),3)<=lowestf)

            lowestf=calcVal(openList(k,1),openList(k,2),3);

            fk=k;

           

        end

    end

   

end% now i have found the lowest f val and its cell

%set equal current and remove remove this lowest f cell from the openList

currentPos= openList(fk,:);

openList(fk,:)=[];

      

                  

%------------------------------------------

%now for neighbours

% generate successors of currentPos

i=-1;

while i<2

    j=-1;

    while j<2

        if(~(i==0 && j==0) && (currentPos(1)+i)>0 && (currentPos(2)+j)>0)

            v = [currentPos(1)+i,currentPos(2)+j]; %v will be the reference here

            %check if the cell is walkable

            if( ~all(cameFrom == v))

                if (Grid(v(1),v(2))==0);%if the grid is walkable

                    disp('calcval being calculate for')

                    v

                %compute its scores

                calcVal(v(1),v(2),1)= abs(v(1)-startPos(1))+abs(v(2)-startPos(2));%calc of g score

                calcVal(v(1),v(2),2)= abs(destinPos(1)-v(1))+abs(destinPos(2)-v(2));%calc of h score

                calcVal(v(1),v(2),3)= calcVal(v(1),v(2),1) + calcVal(v(1),v(2),2); % calc g+h+1 offset

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

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

                end

            end

        end

        j=j+1;

    end

    i=i+1;

end % end of calclulating sucsessors of current cell

calcVal

% for each successor

i=-1;

while i<2

    j=-1;

    while j<2

       if(~(i==0 && j==0) && (currentPos(1)+i)>0 && (currentPos(2)+j)>0)

        nopenlist=1;

        nclosedlist=1;

       

        v = [currentPos(1)+i,currentPos(2)+j]; %v will be the reference here

        %check if the cell is walkable

        if (Grid(v(1),v(2))==0);%if the grid is walkable

            f=calcVal(v(1),v(2),3);

            fval(isnan(fval))=0;

            fval(~fval)=nan;

            b=(fval<f);

            if (destinPos(1)==v(1) && destinPos(2)==v(2))

                stoploop =1;

            end

            if (~isempty(openList))

            for k=1:size(openList,1)%loop throguh openlist

               

                    if (calcVal(openList(k,1),openList(k,2),3)<f)

                        nopenlist=0;

                    end

               

            end

            end

            if (~isempty(closedList))

            for k=1:size(closedList,1) %if closed list has f value lower, skip it

               

                   

                    if(calcVal(closedList(k,1),closedList(k,2),3)<f)

                        nclosedlist=0;

                    end

                   

                                

            end

            end

            if (nopenlist==1 && nclosedlist==1)%otherwise, do not meet the two conditions above

                openList(end+1,:)=v

            end

         

        end

        end

        j=j+1;

    end

    i=i+1;

end % end of calclulating sucsessors of current cell

%add current to closedList

closedList(end+1,:)= currentPos

cameFrom=currentPos;

end%end loop while openList is not empty

calcVal

closedList

openList

       

       

I feel like I have missed the bus somewhere. If I un unable to figure out whats wrong I will just start over, that might be faster. Also I need to prepare for the final presentation. On tue 03.31.2015.