Spice it with some Animation

Ok its not as cool as the title sounds. you can basically watch the progress of the pedestrian as they navigate through obstacles.

04.15.2015

Got an animation to play of the pedestrian pathfinding. A bit of trial and error but now we can introduce behaviour of the pedestrian.

function [  ] = MainFunc( )

%Main function, all other functions will be executed from here

%   Detailed explanation goes here

%for testing get map here

MapGrid=testmap();

%set current position and destination

StartPos=[2 1];

EndPos = [6,3];

%put the location of the person on the grid

MapGrid(StartPos(1),StartPos(2))=2;

%now for the simulation part

simMap=MapGrid;

currentPos=StartPos;

loopC=1;

%plot firt one with handle

%figure(1),clf

%axes('Position',[0,0.25,1,0.5]);

h=pcolor(simMap);

%colormap('cool')

%shading('interp')

%set(h,'EraseMode','xof')

while (loopC==1)

    if (all(currentPos==EndPos))

        loopC=0;

        break;

    end

    %get next step

    [nextPos pathList]=a_star5(MapGrid,currentPos,EndPos);

    %put the current Pos into the map

    simMap(currentPos(1),currentPos(2))=2;

    set(h,'CData',simMap);

    drawnow

   

    %now take out the currentPos

    simMap(currentPos(1),currentPos(2))=0;

     %assign the next step

     currentPos=nextPos;

     nextPos

    

     for j=1:10000000,

         x(j) = j*exp(pi);

     end

    

    

end

end

the last for loop is just to slow down the animation, as its close to the deadline of thi project. I will hold off on the behaviour aspect and start writing up my report first. The details of the code will be on the report

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

Need to Rework My Path finding!

04.03.2015

With Easter weekend I want to buckle down and look at what I am doing wrong. So I re wrote my pseudo code.

a-star psudo code

map info

startPos

destinPos

currentPos

calcVal – multidim matrix with all the calculated values in it

openlist – matrix will contain the row vecotors as a list

closedlist

put startPos into the openList and set f value of startPos to zero

do loop while openlist is not empty and while looping is true

                -get the vector with the lowest f from the openlist and set it to currentPos

                -get the neighbours of currentPos and for each neighbour and while looping is true

                                Set m=movementcost to neighbour

                                Set nx,ny as the neighbour loc

                                -check if neighbour is in the grid

                                                -no don’t don’t do the other checks

-yes, check if neighbour is destPos –yes, set parant into of destPos and set                                                                    looping to false

 check if neighbour is walkable

                                                                -no, nothing

                                                                -yes, check if its in the closed list

                                                                                -yes, nothing

                                                                                -no, check if its in the openlist

                                                                                                -yes, check if m+g of current<g of n

                                                                                                                -no, nothing

                                                                                                                -yes, change parent of n to current

                                                                                                -no, calculate vals and add to openlist

                -del currentnode from openlist and add current node to closed list.

End while loop

Which are the functions that need to be written.

Check if node is in the grid

Check if node is walkable

Check if node is in the list             

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.