Jump to content
  • Advertisement
Sign in to follow this  
mokarrabin

astar path finding code

This topic is 3822 days old which is more than the 365 day threshold we allow for new replies. Please post a new topic.

If you intended to correct an error in the post then please contact us.

Recommended Posts

// ptest.cpp : Defines the entry point for the console application. // This is the fixed code with Adam_42's advise.. #include "stdafx.h" #include <ctime> #include <cstdlib> //////////////////////////////////////////////////// struct CellDatas { int OCList; int GCost; int HCost; int FCost; int cdParentX; int cdParentY; bool Wall; bool DrawPath; }; struct BinHeapData { int Score; int X; int Y; }; class BinaryHeap { private: int hSize; //Size of the heap array BinHeapData Heap[9999]; //Heap Array public: BinaryHeap(){ hSize=0; } int Count() { return hSize; } void setCount(int temphc){ hSize=temphc; } int GetX() { return Heap[1].X; } //Return Y position int GetY() { return Heap[1].Y; } //Return the lowest score int GetScore() { return Heap[1].Score; } //Reset the heap void ReseHeap() { hSize = 0; // ERROR: Not supported in C//#: ReDimStatement } //Remove the Root Object from the heap void RemoveRoot() { //If only the root exists if (hSize <= 1) { hSize = 0; // ERROR: Not supported in C//#: ReDimStatement return; // TODO: might not be correct. Was : Exit Sub } //First copy the very bottom object to the top Heap[1] = Heap[hSize]; //Resize the count hSize=hSize-1; //Shrink the array // ERROR: Not supported in C//#: ReDimStatement //Sort the top item to it's correct position int Parent = 1; int ChildIndex = 1; //Sink the item to it's correct location while (true) { ChildIndex = Parent; if (2 * ChildIndex + 1 <= hSize) { //Find the lowest value of the 2 child nodes if (Heap[ChildIndex].Score >= Heap[2 * ChildIndex].Score) Parent = 2 * ChildIndex; if (Heap[Parent].Score >= Heap[2 * ChildIndex + 1].Score) Parent = 2 * ChildIndex + 1; } else { //Just process the one node if (2 * ChildIndex <= hSize) { if (Heap[ChildIndex].Score >= Heap[2 * ChildIndex].Score) Parent = 2 * ChildIndex; } } //Swap out the child/parent if (Parent != ChildIndex) { BinHeapData tHeap = Heap[ChildIndex]; Heap[ChildIndex] = Heap[Parent]; Heap[Parent] = tHeap; } else { break; } } } //Add the new element to the heap void Add(int inScore, int inX, int inY) { //**We will be ignoring the (0) place in the heap array because //**it's easier to handle the heap with a base of (1..?) //Increment the array count hSize=hSize+1; //Make room in the array // ERROR: Not supported in C//#: ReDimStatement //Store the data Heap[hSize].Score = inScore; Heap[hSize].X = inX; Heap[hSize].Y = inY; //Bubble the item to its correct location int sPos = hSize; while (sPos != 1) { if (Heap[sPos].Score <= Heap[sPos / 2].Score) { BinHeapData tHeap = Heap[sPos / 2]; Heap[sPos / 2] = Heap[sPos]; Heap[sPos] = tHeap; sPos /= 2; } else { break; } } } }; //00000000000000000000000000000000000000000000000000 const inOpened = 1; const inClosed = 2; BinaryHeap HeapClass; int StartX; int StartY; int EndX; int EndY; const xMax=24,yMax=24; CellDatas Map[xMax+1][yMax+1]; bool PathFound; bool PathHunt; int ParentX; int ParentY; bool c,pfdebug=false; int MathAbs(int valtoAbs) // returns the positive value of a number. +5 or - 5 -> 5 { if(valtoAbs<0)return (-1*valtoAbs); else return valtoAbs; } void CleanUp() { int xCnt1, yCnt1; HeapClass.ReseHeap(); for (yCnt1 = 0; yCnt1 <= yMax; yCnt1++) { for (xCnt1 = 0; xCnt1 <= xMax; xCnt1++) { Map[xCnt1][yCnt1].DrawPath = false; Map[xCnt1][yCnt1].FCost = 0; Map[xCnt1][yCnt1].GCost = 0; Map[xCnt1][yCnt1].HCost = 0; Map[xCnt1][yCnt1].OCList = 0; Map[xCnt1][yCnt1].cdParentX = 0; Map[xCnt1][yCnt1].cdParentY = 0; Map[xCnt1][yCnt1].Wall = false; } } } void FindPath() { int xCnt; int yCnt; //Set the flags PathFound = false; PathHunt = true; //Make sure the starting point and ending point are not the same if ((StartX == EndX) && (StartY == EndY)) return; //Make sure the starting point is not a wall if (Map[StartX][StartY].Wall) return; if (Map[EndX][EndY].Wall) return; //HeapClass.ReseHeap(); //Put the starting point on the open list Map[StartX][ StartY].OCList = inOpened; HeapClass.Add(0, StartX, StartY); //Find the children while (PathHunt) { if (HeapClass.Count() != 0) { //Get the parent node ParentX = HeapClass.GetX(); ParentY = HeapClass.GetY(); //Remove the root Map[ParentX][ ParentY].OCList = inClosed; HeapClass.RemoveRoot(); //Find the available children to add to the open list for (yCnt = (ParentY - 1); yCnt <= (ParentY + 1); yCnt++) { for (xCnt = (ParentX - 1); xCnt <= (ParentX + 1); xCnt++) { //Make sure we are not out of bounds if ((xCnt != -1) && (xCnt != xMax+1) && (yCnt != -1) && (yCnt < yMax+1)) { //Make sure it's not on the closed list if (Map[xCnt][yCnt].OCList != inClosed) { //Make sure no wall if (Map[xCnt][yCnt].Wall == false) { //Don't cut across corners bool CanWalk = true; if (xCnt == ParentX - 1) { if (yCnt == ParentY - 1) { if ((Map[ParentX - 1][ParentY].Wall == true) || (Map[ParentX][ParentY - 1].Wall == true)) CanWalk = false; } else if (yCnt == ParentY + 1) { if ((Map[ParentX][ParentY + 1].Wall == true) || (Map[ParentX - 1][ParentY].Wall == true)) CanWalk = false; } } else if (xCnt == ParentX + 1) { if (yCnt == ParentY - 1) { if ((Map[ParentX][ParentY - 1].Wall == true) || (Map[ParentX + 1][ParentY].Wall == true)) CanWalk = false; } else if (yCnt == ParentY + 1) { if ((Map[ParentX + 1][ ParentY].Wall == true) || (Map[ParentX][ ParentY + 1].Wall == true)) CanWalk = false; } } //If we can move this way if (CanWalk == true) { if (Map[xCnt][yCnt].OCList != inOpened) { /**/ //Calculate the GCost if ((MathAbs(xCnt - ParentX) == 1) && (MathAbs(yCnt - ParentY) == 1)) { Map[xCnt][yCnt].GCost = Map[ParentX][ParentY].GCost + 14; } else { Map[xCnt][yCnt].GCost = Map[ParentX][ParentY].GCost + 10; } //Calculate the HCost Map[xCnt][yCnt].HCost = 10 * (MathAbs(xCnt - EndX) + MathAbs(yCnt - EndY)); Map[xCnt][yCnt].FCost = (Map[xCnt][yCnt].GCost + Map[xCnt][yCnt].HCost); if (pfdebug) printf(" - A1>:%d -",HeapClass.Count()); if (pfdebug) printf(" - {%d}{%d}{P:%d} -",xCnt,yCnt,ParentX); //int temphc=HeapClass.Count(); //Add the parent value Map[xCnt][yCnt].cdParentX = ParentX; // <--Trouble Map[xCnt][yCnt].cdParentY = ParentY; //HeapClass.setCount(temphc); if (pfdebug) printf(" - A2>:%d - ",HeapClass.Count()); //Add the item to the heap HeapClass.Add(Map[xCnt][yCnt].FCost, xCnt, yCnt); //Add the item to the open list Map[xCnt][yCnt].OCList = inOpened; } else { //We will check for better value int AddedGCost; if ((MathAbs(xCnt - ParentX) == 1) && (MathAbs(yCnt - ParentY) == 1)) { AddedGCost = 14; } else { AddedGCost = 10; } int tempCost = Map[ParentX][ParentY].GCost + AddedGCost; if (tempCost < Map[xCnt][yCnt].GCost) { Map[xCnt][yCnt].GCost = tempCost; Map[xCnt][yCnt].cdParentX = ParentX; Map[xCnt][yCnt].cdParentY = ParentY; if (Map[xCnt][yCnt].OCList == inOpened) { int NewCost = Map[xCnt][yCnt].HCost + Map[xCnt][yCnt].GCost; if (pfdebug) printf(" - A3> :%d\n",HeapClass.Count()); HeapClass.Add(NewCost, xCnt, yCnt); } } } } } } } } } } else { PathFound = false; PathHunt = false; return; } //If we find a path if (Map[EndX][EndY].OCList == inOpened) { PathFound = true; PathHunt = false; } } ///'''''''''''''''''''''''''''''''''''''''''''''''''''''' // Reverse the path info and put in a 2d array from start to end destination coordinates /* if (PathFound) { pathcount = 0; pathInfoX(pathcount) = EndX; pathInfoY(pathcount) = EndY; pathcount = 1; int tX = EndX; int tY = EndY; while (true) { int sX = Map(tX, tY).cdParentX; int sY = Map(tX, tY).cdParentY; pathInfoX(pathcount) = sX; pathInfoY(pathcount) = sY; tX = sX; tY = sY; if (tX == StartX & tY == StartY) break; pathcount = pathcount + 1; } CleanUp(); } */ } //////////////////////////////////////////////////// int main(int argc, char* argv[]) { int pathcount=0,index=0; int pathInfoX[9999],pathInfoY[9999]; /* StartX = 12; StartY = 12; EndX = 16; EndY = 16; Map[11][11].Wall = true;Map[12][11].Wall = false;Map[13][11].Wall = true; Map[11][13].Wall = true;Map[12][13].Wall = true;Map[13][13].Wall = true; Map[11][12].Wall = true;Map[13][12].Wall = true; printf("Calling FindPath()\n"); FindPath(); printf("End FindPath()\n\n"); if (PathFound) { // put the path info and put in a 2d array pathcount = 0; pathInfoX[pathcount] = EndX; pathInfoY[pathcount] = EndY; pathcount = 1; int tX = EndX; int tY = EndY; while (true) { int sX = Map[tX][tY].cdParentX; int sY = Map[tX][tY].cdParentY; pathInfoX[pathcount] = sX; pathInfoY[pathcount] = sY; tX = sX; tY = sY; if (tX == StartX && tY == StartY) break; pathcount++; } CleanUp(); } if (PathFound) { printf("Wow, Path found!\n"); int i; for(i=pathcount;i>-1;i--) { printf("x[%d],y[%d]\n",pathInfoX,pathInfoY); } } else { printf("Damn, Path not found!\n"); } //*////////////////////////// /**/ printf("\n\n\n*********************************\n\n\n"); int zoom; for(zoom=1;zoom<2;zoom++){ printf("zoom: %d\n",zoom); printf("\n\n\n*********** TEST AGAIN ********\n\n\n"); srand(time(0)); StartX = int (rand() % 24); StartY = int (rand() % 24); EndX = int (rand() % 24); EndY = int (rand() % 24); /* StartX = 24; StartY = 24; EndX = 0;//6; EndY = 0;//17; */ printf("Start:[%d,%d], End:[%d,%d]\n",StartX,StartY,EndX,EndY); /* Map[0][1].Wall = true; Map[17][17].Wall = true;Map[12][11].Wall = false;Map[13][11].Wall = true; Map[20][10].Wall = true;Map[12][13].Wall = true;Map[13][13].Wall = true; Map[11][12].Wall = true;Map[13][12].Wall = true; */ printf("Calling FindPath()\n"); FindPath(); printf("End FindPath()\n\n"); /**/ if (PathFound) { // put the path info and put in a 2d array pathcount = 0; pathInfoX[pathcount] = EndX; pathInfoY[pathcount] = EndY; pathcount = 1; int tX = EndX; int tY = EndY; while (true) { int sX = Map[tX][tY].cdParentX; int sY = Map[tX][tY].cdParentY; pathInfoX[pathcount] = sX; pathInfoY[pathcount] = sY; tX = sX; tY = sY; //if (pathcount<5){printf("tX:%d,tY:%d\n",tX,tY);} if ((tX == StartX) && (tY == StartY)) break; pathcount++; //if (pathcount>50)break; //printf("pathcount:[%d]\n",pathcount); } CleanUp(); } if (PathFound) { printf("Wow, Path found!\n"); int i; for(i=pathcount;i>-1;i--) { printf("x[%d],y[%d]\n",pathInfoX,pathInfoY); } } else { printf("Damn, Path not found!\n"); } } //////////////////////////////// return 0; } [Edited by - mokarrabin on April 6, 2008 1:16:49 AM]

Share this post


Link to post
Share on other sites
Advertisement
Please use [ code] or [ source ] [/source] tags when posting source code.

Why did you post this?

Share this post


Link to post
Share on other sites
Quote:
Original post by Hodgman
Please use [ code] or [ source ] [/source] tags when posting source code.

Why did you post this?


Some help would be good. There is a bug in the code that is really weird.


The variable hSize is supposed to be incremented by 1 when a new element is added to the heap. However when findpath is running that variable jumps by 10+ without any reason. it happensa ater the line that says [//Add the parent value]

And this does not happen always.
But when StartX = 23;
StartY = 1;
EndX = 10;//6;
EndY = 17;//17;

Any ideas ?

Share this post


Link to post
Share on other sites
Your gonna have to narrow down the problem, nobody is going to read 6 pages of code for you..

Also step trough it with a debugger, see where it gets incremented.

Share this post


Link to post
Share on other sites
Are you clearing the memory, pointers etc correctly?
Are you doing debugging in an isolated environment to prevent memoryleak/overwriting from other functions?

I had a bug in my texture manager that randomly destroyed my pathfinding - I went through hell to find that. :D

Share this post


Link to post
Share on other sites
Quote:
Original post by mokarrabin
The variable hSize is supposed to be incremented by 1 when a new element is added to the heap. However when findpath is running that variable jumps by 10+ without any reason. it happensa ater the line that says [//Add the parent value]

And this does not happen always.
But when StartX = 23;
StartY = 1;
EndX = 10;//6;
EndY = 17;//17;

Any ideas ?


So the problem is with this bit of code:


//Add the parent value
Map[xCnt][yCnt].ParentX = ParentX;
Map[xCnt][yCnt].ParentY = ParentY;
printf(" - A2>:%d - ",HeapClass.Count());



I'd assume that yCnt or xCnt (or both) are either < 0 or >= 24 which means they are accessing data outside of the array bounds and trashing your hSize variable.

The obvious fix is to change:

const xMax=24,yMax=24;
CellData Map[xMax][yMax];

to:

const xMax=24,yMax=24;
CellData Map[xMax+1][yMax+1];

Since this code is clearly converted from a language where arrays are 1-based not 0-based.

Share this post


Link to post
Share on other sites
yah, i used a translator to change from vb.net
it seems to work now, thanks a lot. I updated the original code with ur fix, however i dont understand how it works now really but it does ! lol till it crashes again... hope not..

I am also posting the original code by the original autoher, which got transferred to vb.net and then to this... heh he..


// starpath.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include <stdlib.h>
#include <windows.h>
#include <mmsystem.h>
#include <ctime> // For time() random number gen.
//#include <cstdlib> // For srand() and rand()


#pragma comment( lib, "winmm.lib" ) // for gettime function.

//-----------------------------------------------------------------------------
// Global variables and constants
//-----------------------------------------------------------------------------
char smileyActivated = 0;
int StartxLoc; int StartyLoc; int speed [4];
int EndxLoc; int EndyLoc;
int g_showDirections=0;
float searchTime;
int successfulPaths=0;

/*
;===================================================================
;A* Pathfinder (Version 1.71a) by Patrick Lester. Used by permission.
;===================================================================
;Last updated 06/16/03 -- Visual C++ version
*/

//Declare constants
const mapWidth = 25, mapHeight = 25, tileSize = 1, numberPeople = 3;
int onClosedList = 10;
const notfinished = 0, notStarted = 0;// path-related constants
const found = 1, nonexistent = 2;
const walkable = 0, unwalkable = 1;// walkability array constants

//Create needed arrays
char walkability [mapWidth][mapHeight];
int openList[mapWidth*mapHeight+2]; //1 dimensional array holding ID# of open list items
int whichList[mapWidth+1][mapHeight+1]; //2 dimensional array used to record
// whether a cell is on the open list or on the closed list.
int openX[mapWidth*mapHeight+2]; //1d array stores the x location of an item on the open list
int openY[mapWidth*mapHeight+2]; //1d array stores the y location of an item on the open list
int parentX[mapWidth+1][mapHeight+1]; //2d array to store parent of each cell (x)
int parentY[mapWidth+1][mapHeight+1]; //2d array to store parent of each cell (y)
int Fcost[mapWidth*mapHeight+2]; //1d array to store F cost of a cell on the open list
int Gcost[mapWidth+1][mapHeight+1]; //2d array to store G cost for each cell.
int Hcost[mapWidth*mapHeight+2]; //1d array to store H cost of a cell on the open list
int pathLength[numberPeople+1]; //stores length of the found path for critter
//int pathLocation[numberPeople+1]; //stores current position along the chosen path for critter
int* pathBank [numberPeople+1];

//Path reading variables
int pathStatus[numberPeople+1];
int xPath[numberPeople+1];
int yPath[numberPeople+1];

//-----------------------------------------------------------------------------
// Name: InitializePathfinder
// Desc: Allocates memory for the pathfinder.
//-----------------------------------------------------------------------------
void InitializePathfinder (void)
{
for (int x = 0; x < numberPeople+1; x++)
pathBank [x] = (int*) malloc(4);
}


//-----------------------------------------------------------------------------
// Name: EndPathfinder
// Desc: Frees memory used by the pathfinder.
//-----------------------------------------------------------------------------
void EndPathfinder (void)
{
for (int x = 0; x < numberPeople+1; x++)
{
free (pathBank [x]);
}
}


//-----------------------------------------------------------------------------
// Name: FindPath
// Desc: Finds a path using A*
//-----------------------------------------------------------------------------
int FindPath (int pathfinderID,int startingX, int startingY, int targetX, int targetY)
{
int onOpenList=0, parentXval=0, parentYval=0,
a=0, b=0, m=0, u=0, v=0, temp=0, corner=0, numberOfOpenListItems=0,
addedGCost=0, tempGcost = 0, path = 0,
tempx, pathX, pathY, cellPosition,
newOpenListItemID=0;

//1. Convert location data (in pixels) to coordinates in the walkability array.
int startX = startingX/tileSize;
int startY = startingY/tileSize;
targetX = targetX/tileSize;
targetY = targetY/tileSize;

//2.Quick Path Checks: Under the some circumstances no path needs to
// be generated ...

// If starting location and target are in the same location...
if (startX == targetX && startY == targetY)// && pathLocation[pathfinderID] > 0)
return found;
if (startX == targetX && startY == targetY)// && pathLocation[pathfinderID] == 0)
return nonexistent;

// If target square is unwalkable, return that it's a nonexistent path.
if (walkability[targetX][targetY] == unwalkable)
goto noPath;

//3.Reset some variables that need to be cleared
if (onClosedList > 1000000) //reset whichList occasionally
{
for (int x = 0; x < mapWidth;x++) {
for (int y = 0; y < mapHeight;y++)
whichList [x][y] = 0;
}
onClosedList = 10;
}
onClosedList = onClosedList+2; //changing the values of onOpenList and onClosed list is faster than redimming whichList() array
onOpenList = onClosedList-1;
pathLength [pathfinderID] = notStarted;//i.e, = 0
//pathLocation [pathfinderID] = notStarted;//i.e, = 0
Gcost[startX][startY] = 0; //reset starting square's G value to 0

//4.Add the starting location to the open list of squares to be checked.
numberOfOpenListItems = 1;
openList[1] = 1;//assign it as the top (and currently only) item in the open list, which is maintained as a binary heap (explained below)
openX[1] = startX ; openY[1] = startY;

//5.Do the following until a path is found or deemed nonexistent.
do
{

//6.If the open list is not empty, take the first cell off of the list.
// This is the lowest F cost cell on the open list.
if (numberOfOpenListItems != 0)
{

//7. Pop the first item off the open list.
parentXval = openX[openList[1]];
parentYval = openY[openList[1]]; //record cell coordinates of the item
whichList[parentXval][parentYval] = onClosedList;//add the item to the closed list

// Open List = Binary Heap: Delete this item from the open list, which
// is maintained as a binary heap. For more information on binary heaps, see:
// http://www.policyalmanac.org/games/binaryHeaps.htm
numberOfOpenListItems = numberOfOpenListItems - 1;//reduce number of open list items by 1

// Delete the top item in binary heap and reorder the heap, with the lowest F cost item rising to the top.
openList[1] = openList[numberOfOpenListItems+1];//move the last item in the heap up to slot #1
v = 1;

// Repeat the following until the new item in slot #1 sinks to its proper spot in the heap.
do
{
u = v;
if (2*u+1 <= numberOfOpenListItems) //if both children exist
{
//Check if the F cost of the parent is greater than each child.
//Select the lowest of the two children.
if (Fcost[openList] >= Fcost[openList[2*u]])
v = 2*u;
if (Fcost[openList[v]] >= Fcost[openList[2*u+1]])
v = 2*u+1;
}
else
{
if (2*u <= numberOfOpenListItems) //if only child #1 exists
{
//Check if the F cost of the parent is greater than child #1
if (Fcost[openList] >= Fcost[openList[2*u]])
v = 2*u;
}
}

if (u != v) //if parent's F is > one of its children, swap them
{
temp = openList;
openList = openList[v];
openList[v] = temp;
}
else
break; //otherwise, exit loop

}
while (1);//reorder the binary heap


//7.Check the adjacent squares. (Its "children" -- these path children
// are similar, conceptually, to the binary heap children mentioned
// above, but don't confuse them. They are different. Path children
// are portrayed in Demo 1 with grey pointers pointing toward
// their parents.) Add these adjacent child squares to the open list
// for later consideration if appropriate (see various if statements
// below).
for (b = parentYval-1; b <= parentYval+1; b++){
for (a = parentXval-1; a <= parentXval+1; a++){

// If not off the map (do this first to avoid array out-of-bounds errors)
if (a != -1 && b != -1 && a != mapWidth && b != mapHeight){

// If not already on the closed list (items on the closed list have
// already been considered and can now be ignored).
if (whichList[a] != onClosedList) {

// If not a wall/obstacle square.
if (walkability [a] != unwalkable) {

// Don't cut across corners
corner = walkable;
if (a == parentXval-1)
{
if (b == parentYval-1)
{
if (walkability[parentXval-1][parentYval] == unwalkable
|| walkability[parentXval][parentYval-1] == unwalkable) corner = unwalkable;
}
else if (b == parentYval+1)
{
if (walkability[parentXval][parentYval+1] == unwalkable
|| walkability[parentXval-1][parentYval] == unwalkable)
corner = unwalkable;
}
}
else if (a == parentXval+1)
{
if (b == parentYval-1)
{
if (walkability[parentXval][parentYval-1] == unwalkable
|| walkability[parentXval+1][parentYval] == unwalkable)
corner = unwalkable;
}
else if (b == parentYval+1)
{
if (walkability[parentXval+1][parentYval] == unwalkable
|| walkability[parentXval][parentYval+1] == unwalkable)
corner = unwalkable;
}
}
if (corner == walkable) {

// If not already on the open list, add it to the open list.
if (whichList[a] != onOpenList)
{

//Create a new open list item in the binary heap.
newOpenListItemID = newOpenListItemID + 1; //each new item has a unique ID #
m = numberOfOpenListItems+1;
openList[m] = newOpenListItemID;//place the new open list item (actually, its ID#) at the bottom of the heap
openX[newOpenListItemID] = a;
openY[newOpenListItemID] = b;//record the x and y coordinates of the new item

//Figure out its G cost
if (abs(a-parentXval) == 1 && abs(b-parentYval) == 1)
addedGCost = 14;//cost of going to diagonal squares
else
addedGCost = 10;//cost of going to non-diagonal squares
Gcost[a] = Gcost[parentXval][parentYval] + addedGCost;

//Figure out its H and F costs and parent
Hcost[openList[m]] = 10*(abs(a - targetX) + abs(b - targetY));
Fcost[openList[m]] = Gcost[a] + Hcost[openList[m]];
parentX[a] = parentXval ; parentY[a] = parentYval;

//Move the new open list item to the proper place in the binary heap.
//Starting at the bottom, successively compare to parent items,
//swapping as needed until the item finds its place in the heap
//or bubbles all the way to the top (if it has the lowest F cost).
while (m != 1) //While item hasn't bubbled to the top (m=1)
{
//Check if child's F cost is < parent's F cost. If so, swap them.
if (Fcost[openList[m]] <= Fcost[openList[m/2]])
{
temp = openList[m/2];
openList[m/2] = openList[m];
openList[m] = temp;
m = m/2;
}
else
break;
}
numberOfOpenListItems = numberOfOpenListItems+1;//add one to the number of items in the heap

//Change whichList to show that the new item is on the open list.
whichList[a] = onOpenList;
}

//8.If adjacent cell is already on the open list, check to see if this
// path to that cell from the starting location is a better one.
// If so, change the parent of the cell and its G and F costs.
else //If whichList(a,b) = onOpenList
{

//Figure out the G cost of this possible new path
if (abs(a-parentXval) == 1 && abs(b-parentYval) == 1)
addedGCost = 14;//cost of going to diagonal tiles
else
addedGCost = 10;//cost of going to non-diagonal tiles
tempGcost = Gcost[parentXval][parentYval] + addedGCost;

//If this path is shorter (G cost is lower) then change
//the parent cell, G cost and F cost.
if (tempGcost < Gcost[a]) //if G cost is less,
{
parentX[a] = parentXval; //change the square's parent
parentY[a] = parentYval;
Gcost[a] = tempGcost;//change the G cost

//Because changing the G cost also changes the F cost, if
//the item is on the open list we need to change the item's
//recorded F cost and its position on the open list to make
//sure that we maintain a properly ordered open list.
for (int x = 1; x <= numberOfOpenListItems; x++) //look for the item in the heap
{
if (openX[openList[x]] == a && openY[openList[x]] == b) //item found
{
Fcost[openList[x]] = Gcost[a] + Hcost[openList[x]];//change the F cost

//See if changing the F score bubbles the item up from it's current location in the heap
m = x;
while (m != 1) //While item hasn't bubbled to the top (m=1)
{
//Check if child is < parent. If so, swap them.
if (Fcost[openList[m]] < Fcost[openList[m/2]])
{
temp = openList[m/2];
openList[m/2] = openList[m];
openList[m] = temp;
m = m/2;
}
else
break;
}
break; //exit for x = loop
} //If openX(openList(x)) = a
} //For x = 1 To numberOfOpenListItems
}//If tempGcost < Gcost(a,b)

}//else If whichList(a,b) = onOpenList
}//If not cutting a corner
}//If not a wall/obstacle square.
}//If not already on the closed list
}//If not off the map
}//for (a = parentXval-1; a <= parentXval+1; a++){
}//for (b = parentYval-1; b <= parentYval+1; b++){

}//if (numberOfOpenListItems != 0)

//9.If open list is empty then there is no path.
else
{
path = nonexistent; break;
}

//If target is added to open list then path has been found.
if (whichList[targetX][targetY] == onOpenList)
{
path = found; break;
}

}
while (1);//Do until path is found or deemed nonexistent

//10.Save the path if it exists.
if (path == found)
{

//a.Working backwards from the target to the starting location by checking
// each cell's parent, figure out the length of the path.
pathX = targetX; pathY = targetY;
do
{
//Look up the parent of the current cell.
tempx = parentX[pathX][pathY];
pathY = parentY[pathX][pathY];
pathX = tempx;

//Figure out the path length
pathLength[pathfinderID] = pathLength[pathfinderID] + 1;
}
while (pathX != startX || pathY != startY);

//b.Resize the data bank to the right size in bytes
pathBank[pathfinderID] = (int*) realloc (pathBank[pathfinderID],
pathLength[pathfinderID]*8);

//c. Now copy the path information over to the databank. Since we are
// working backwards from the target to the start location, we copy
// the information to the data bank in reverse order. The result is
// a properly ordered set of path data, from the first step to the
// last.
pathX = targetX ; pathY = targetY;
cellPosition = pathLength[pathfinderID]*2;//start at the end
do
{
cellPosition = cellPosition - 2;//work backwards 2 integers
pathBank[pathfinderID] [cellPosition] = pathX;
pathBank[pathfinderID] [cellPosition+1] = pathY;

//d.Look up the parent of the current cell.
tempx = parentX[pathX][pathY];
pathY = parentY[pathX][pathY];
pathX = tempx;

//e.If we have reached the starting square, exit the loop.
}
while (pathX != startX || pathY != startY);

//11.Read the first path step into xPath/yPath arrays
//ReadPath(pathfinderID,startingX,startingY,1);

}
return path;


//13.If there is no path to the selected target, set the pathfinder's
// xPath and yPath equal to its current location and return that the
// path is nonexistent.
noPath:
xPath[pathfinderID] = startingX;
yPath[pathfinderID] = startingY;
return nonexistent;
}

void LoadMapData (void)
{
//initialize the map to completely walkable
{
for (int x = 0; x <= mapWidth-1; x++){
for (int y = 0; y <= mapHeight-1; y++){
walkability [x][y] = walkable;
}}
}

//put some wall
/*srand(time(0)); // Initialize random number generator.
for (int x = 0; x <= 503; x++){
walkability [((rand() % mapWidth-1) + 1)][((rand() % mapHeight-1) + 1)]=unwalkable;
}*/
}

void ShowPath(int ID)
{
int xLoc; int yLoc; int speed;

printf("ID: [%d], pathLength[%d]\n",ID,pathLength[ID]);

int i,lx,ly;
for (i=1;i<=pathLength[ID];i++) // excluding current location , startx,starty
{
lx = pathBank[ID][i*2-2];
ly = pathBank[ID][i*2-1];
printf("Lx:[%d], Ly:[%d]\n",lx,ly);
}
}

bool StartSearch (int ID)
{

//Call the findPath function.
int time1 = timeGetTime();
pathStatus[ID] = FindPath(ID,StartxLoc,StartyLoc,EndxLoc,EndyLoc);
int time2 = timeGetTime();
searchTime = (time2-time1)/1000.0f; //secounds

//If path found!
if (pathStatus[ID] == found)// MoveSprite(ID);
{
printf("Found path!, Time [%f]\n",searchTime);
successfulPaths++;
return true;
}
else
{
printf("Not Found path!\n");
return false;
}
}

int main(int argc, char* argv[])
{
LoadMapData();

srand(time(0)); // Initialize random number generator.

int time1 = timeGetTime();

for (int i=0;i<1;i++)
{
InitializePathfinder();

/*
StartxLoc = (rand() % mapWidth-1) + 1;
StartyLoc = (rand() % mapHeight-1) + 1;
EndxLoc = (rand() % mapWidth-1) + 1;
EndyLoc = (rand() % mapHeight-1) + 1;

*/
StartxLoc = 0;
StartyLoc = 0;
EndxLoc = 1;
EndyLoc = 1;
/*
walkability [10][11]=unwalkable;
walkability [10][11]=unwalkable;
walkability [12][2]=unwalkable;
walkability [12][3]=unwalkable;
walkability [12][4]=unwalkable;
walkability [12][5]=unwalkable;
walkability [12][6]=unwalkable;
walkability [12][7]=unwalkable;
*/
printf("\nRun: [%d] StartLoc[%d,%d],EndLoc:[%d,%d]\n",i,StartxLoc,StartyLoc,EndxLoc,EndyLoc);

bool result = StartSearch(0);
if (result) ShowPath(0);

EndPathfinder();
}

int time2 = timeGetTime();
searchTime = (time2-time1)/1000.0f; //secounds
printf("Total Time : [%f], Total Paths Found: [%d]\n",searchTime,successfulPaths);
/*
//show the grid.
for (int x = 0; x <= mapWidth-1; x++){
for (int y = 0; y <= mapHeight-1; y++){
if ((x==StartxLoc) && (y==StartyLoc))printf("S");
else if ((x==EndxLoc) && (y==EndyLoc))printf("E");
else if(walkability [x][y] == walkable) printf(".");
else printf("+");
}printf("\n");}
*/

return 0;
}

Share this post


Link to post
Share on other sites
Sign in to follow this  

  • Advertisement
×

Important Information

By using GameDev.net, you agree to our community Guidelines, Terms of Use, and Privacy Policy.

We are the game development community.

Whether you are an indie, hobbyist, AAA developer, or just trying to learn, GameDev.net is the place for you to learn, share, and connect with the games industry. Learn more About Us or sign up!

Sign me up!