A* Pathfinding by Phaelax23rd Jun 2013 21:40
|
|---|
|
Summary IanM's path finding code ported over to AGK. Description Read the thread for full details: Code ` This code was downloaded from The Game Creators
` It is reproduced here with full permission
` http://www.thegamecreators.com
/* *************************************************
* Original Author: IanM (DBP)
* Ported to AGK: Phaelax
* Date: June 22, 2013
*
*
* COMMANDS
* ----------------------------------------
*
* CreateSearchMap(width, height)
* CreateSearchPathList(NumberOfPaths)
* SetSearchMap(x, y, value) (values greater than 0 are not walkable)
* GetSearchMap(x, y)
* GetSearchPathX(path, move)
* GetSearchPathY(path, move)
* GetSearchPathSize(path)
* SetSearchRestrictDiagonals(mode) (1 will avoid diagonals between blocked cells)
* SetMaximumCost(cost)
*
* SearchMapAStar4(path, startX, startY, finishX, finishY)
* SearchMapAStar8(path, startX, startY, finishX, finishY)
* SearchMapFlood4(path, startX, startY, finishX, finishY)
* SearchMapFlood8(path, startX, startY, finishX, finishY)
*
* GetFloodCost(finishX, finishY) (Uses previous Flood4 search to identify cost of specified target)
* *************************************************
*/
// ===================================================
// Must call this sub-routine first
// ===================================================
SEARCH_Declare_UDT:
type SEARCH_Position_t
x as integer
y as integer
endtype
type SEARCH_TileInformation_t
status as integer
G as integer
H as integer
F as integer
parentX as integer
parentY as integer
O as integer
endtype
type SEARCH_OpenListItem_t
F as integer
x as integer
y as integer
endtype
RETURN
// ===================================================
// Initialize search variables
// ===================================================
function initPathfinder()
global SEARCH_MapWidth as integer
global SEARCH_MapHeight as integer
global SEARCH_MaxPaths as integer
global SEARCH_CurrentPosition as SEARCH_Position_t
// Used for HEAP format used in A*
global SEARCH_OpenListSize as integer
// Used for flood4 search
global SEARCH_OpenListTop as integer
global SEARCH_OpenListBottom as integer
// Optimization for flood4 search
global SEARCH_PreviousStart as SEARCH_Position_t
// General search parameters
global SEARCH_RestrictedDiagonals as integer
global SEARCH_MaximumCost as integer
global SEARCH_ParametersChanged as integer
global SEARCH_LastSearch as integer
// Initialize flags
global SEARCH_PathsInitialized as integer
global SEARCH_MapInitialized as integer
dim SEARCH_Map[SEARCH_MapWidth, SEARCH_MapHeight] as integer
dim SEARCH_TileInfo[SEARCH_MapWidth, SEARCH_MapHeight] as SEARCH_TileInformation_t
dim SEARCH_OpenList[SEARCH_MapWidth * SEARCH_MapHeight] as SEARCH_OpenListItem_t
dim SEARCH_SearchPath[SEARCH_MaxPaths, SEARCH_MapWidth * SEARCH_MapHeight] as SEARCH_Position_t
dim SEARCH_PathSize[SEARCH_MaxPaths] as integer
endfunction
// ===================================================
//
// ===================================================
function CreateSearchMap(X as integer, Y as integer)
SEARCH_MapWidth = X
SEARCH_MapHeight = Y
SEARCH_PreviousStart.X = -9999
SEARCH_PreviousStart.Y = -9999
undim SEARCH_Map[]
undim SEARCH_TileInfo[]
undim SEARCH_OpenList[]
global dim SEARCH_Map[X, Y] as integer
global dim SEARCH_TileInfo[X, Y] as SEARCH_TileInformation_t
global dim SEARCH_OpenList[X * Y] as SEARCH_OpenListItem_t
SEARCH_MapInitialized = 1
endfunction
// ===================================================
//
// ===================================================
function CreateSearchPathLists(Paths as integer)
SEARCH_MaxPaths = Paths
undim SEARCH_SearchPath[]
undim SEARCH_PathSize[]
global dim SEARCH_SearchPath[SEARCH_MaxPaths, SEARCH_MapWidth * SEARCH_MapHeight] as SEARCH_Position_t
global dim SEARCH_PathSize[SEARCH_MaxPaths] as integer
SEARCH_PathsInitialized = 1
endfunction
// ===================================================
//
// ===================================================
function SetSearchMap(X as integer, Y as integer, Value as integer)
if SEARCH_MapInitialized = 0 then exitfunction
if X < 0 or X >= SEARCH_MapWidth then exitfunction
if Y < 0 or Y >= SEARCH_MapHeight then exitfunction
SEARCH_PreviousStart.X = -9999
SEARCH_Map[X,Y] = Value
endfunction
// ===================================================
//
// ===================================================
function GetSearchMap(X as integer, Y as integer)
if SEARCH_MapInitialized = 0 then exitfunction
if X < 0 or X >= SEARCH_MapWidth then exitfunction 0
if Y < 0 or Y >= SEARCH_MapHeight then exitfunction 0
v = SEARCH_Map[X,Y]
exitfunction v
endfunction 0
// ===================================================
//
// ===================================================
function GetSearchPathX(Path as integer, Move as integer)
if SEARCH_PathsInitialized = 0 then exitfunction -1
if Path < 0 or Path > SEARCH_MaxPaths then exitfunction -1
if Move < 0 or Move > SEARCH_PathSize[Path] then exitfunction -1
x = SEARCH_SearchPath[Path, SEARCH_PathSize[Path]-Move].X
exitfunction x
endfunction -1
// ===================================================
//
// ===================================================
function GetSearchPathY(Path as integer, Move as integer)
if SEARCH_PathsInitialized = 0 then exitfunction -1
if Path < 0 or Path > SEARCH_MaxPaths then exitfunction -1
if Move < 0 or Move > SEARCH_PathSize[Path] then exitfunction -1
y = SEARCH_SearchPath[Path, SEARCH_PathSize[Path]-Move].Y
exitfunction y
endfunction -1
// ===================================================
//
// ===================================================
function GetSearchPathSize(Path as integer)
if SEARCH_PathsInitialized = 0 then exitfunction -1
p = SEARCH_PathSize[Path]
if Path >= 0 or Path <= SEARCH_MaxPaths then exitfunction p
endfunction -1
// ===================================================
//
// ===================================================
function SetSearchRestrictDiagonals(Mode as integer)
if SEARCH_RestrictedDiagonals <> Mode
SEARCH_RestrictedDiagonals = Mode
SEARCH_ParametersChanged = 1
endif
endfunction
// ===================================================
//
// ===================================================
function SetSearchMaximumCost(Cost as integer)
if SEARCH_MaximumCost <> Cost*10
SEARCH_MaximumCost = Cost * 10
SEARCH_ParametersChanged = 1
endif
endfunction
// ===================================================
//
// ===================================================
function GetFloodCost(TX as integer, TY as integer)
if SEARCH_MapInitialized = 0 then exitfunction -1
if SEARCH_PathsInitialized = 0 then exitfunction -1
F = SEARCH_TileInfo[TX,TY].F
if SEARCH_PreviousStart.X <> -9999 then exitfunction F
endfunction -1
// ===================================================
//
// ===================================================
function SEARCH_SingleStepCost(X as integer, Y as integer)
if X = 0 or Y = 0 then exitfunction 10
endfunction 14
// ===================================================
//
// ===================================================
function SEARCH_EstimateDistance(X as integer, Y as integer, TX as integer, TY as integer)
Distance as integer
Distance = (abs(X - TX) + abs(Y - TY))*10
endfunction Distance
// ===================================================
//
// ===================================================
function SEARCH_InitializeTileInfo(SX as integer, SY as integer)
SEARCH_OpenListSize=0
SEARCH_PopulateTileInfo()
SEARCH_TileInfo[SX, SY].G = 0
SEARCH_TileInfo[SX, SY].H = SEARCH_EstimateDistance(SX, SY, TX, TY)
SEARCH_TileInfo[SX, SY].F = SEARCH_TileInfo[SX, SY].H
SEARCH_TileInfo[SX, SY].Status = 1
SEARCH_TileInfo[SX, SY].ParentX = SX
SEARCH_TileInfo[SX, SY].ParentY = SY
SEARCH_AddToOpenList(SX, SY)
endfunction
// ===================================================
//
// ===================================================
function SEARCH_PopulateTileInfo()
for X = 0 to SEARCH_MapWidth
for Y = 0 to SEARCH_MapHeight
SEARCH_TileInfo[X,Y].F = 0
if SEARCH_Map[X,Y] > 0
SEARCH_TileInfo[X,Y].Status = 2
else
SEARCH_TileInfo[X,Y].Status = 0
endif
next Y
next X
endfunction
// ===================================================
//
// ===================================================
function SEARCH_AddToOpenList(X as integer, Y as integer)
Parent as integer
Child as integer
Cost as integer
Child = SEARCH_OpenListSize
inc SEARCH_OpenListSize
Cost = SEARCH_TileInfo[X,Y].F
do
if Child <= 0 then exit
Parent = (Child - 1)/2
if SEARCH_OpenList[Parent].F < Cost then exit
SEARCH_OpenList[Child].F = SEARCH_OpenList[Parent].F
SEARCH_OpenList[Child].X = SEARCH_OpenList[Parent].X
SEARCH_OpenList[Child].Y = SEARCH_OpenList[Parent].Y
Child = Parent
loop
SEARCH_OpenList[Child].F = Cost
SEARCH_OpenList[Child].X = X
SEARCH_OpenList[Child].Y = Y
endfunction
// ===================================================
//
// ===================================================
function SEARCH_GetLowestCostOpen()
Parent as integer
Child as integer
Cost as integer
if SEARCH_OpenListSize <= 0 then exitfunction 0
dec SEARCH_OpenListSize
SEARCH_CurrentPosition.X = SEARCH_OpenList[0].X
SEARCH_CurrentPosition.Y = SEARCH_OpenList[0].Y
Cost = SEARCH_OpenList[SEARCH_OpenListSize].F
X = SEARCH_OpenList[SEARCH_OpenListSize].X
Y = SEARCH_OpenList[SEARCH_OpenListSize].Y
Parent = 0
do
Child = (2 * Parent) + 1
if Child >= SEARCH_OpenListSize then exit
if Child+1 < SEARCH_OpenListSize
if SEARCH_OpenList[Child].F > SEARCH_OpenList[Child + 1].F then inc Child
endif
if SEARCH_OpenList[Child].F < Cost
SEARCH_OpenList[Parent].F = SEARCH_OpenList[Child].F
SEARCH_OpenList[Parent].X = SEARCH_OpenList[Child].X
SEARCH_OpenList[Parent].Y = SEARCH_OpenList[Child].Y
Parent = Child
else
exit
endif
loop
SEARCH_OpenList[Parent].F = Cost
SEARCH_OpenList[Parent].X = X
SEARCH_OpenList[Parent].Y = Y
if SEARCH_TileInfo[SEARCH_CurrentPosition.X,SEARCH_CurrentPosition.Y].Status = 2 then exitfunction SEARCH_GetLowestCostOpen()
SEARCH_TileInfo[SEARCH_CurrentPosition.X,SEARCH_CurrentPosition.Y].Status = 2
endfunction 1
// ===================================================
//
// ===================================================
function SEARCH_QueueOntoOpen(X as integer, Y as integer)
inc SEARCH_OpenListTop
SEARCH_OpenList[SEARCH_OpenListTop].X = X
SEARCH_OpenList[SEARCH_OpenListTop].Y = Y
endfunction
// ===================================================
//
// ===================================================
function SEARCH_UnqueueFromOpen()
if SEARCH_OpenListBottom => SEARCH_OpenListTop then exitfunction 0
inc SEARCH_OpenListBottom
SEARCH_CurrentPosition.X = SEARCH_OpenList[SEARCH_OpenListBottom].X
SEARCH_CurrentPosition.Y = SEARCH_OpenList[SEARCH_OpenListBottom].Y
endfunction 1
// ===================================================
//
// ===================================================
function SEARCH_BuildPath(Path as integer, SX as integer, SY as integer, TX as integer, TY as integer)
Moves = 0
X = TX
Y = TY
SEARCH_SearchPath[Path, Moves].X = X
SEARCH_SearchPath[Path, Moves].Y = Y
repeat
inc Moves
SEARCH_SearchPath[Path, Moves].X = SEARCH_TileInfo[X,Y].ParentX
SEARCH_SearchPath[Path, Moves].Y = SEARCH_TileInfo[X,Y].ParentY
X = SEARCH_SearchPath[Path, Moves].X
Y = SEARCH_SearchPath[Path, Moves].Y
until X = SX and Y = SY
SEARCH_PathSize[Path] = Moves
endfunction Moves
// ===================================================
//
// ===================================================
function SEARCH_ClearPath(Path as integer, SX as integer, SY as integer)
SEARCH_SearchPath[Path, 0].X = SX
SEARCH_SearchPath[Path, 1].Y = SY
SEARCH_PathSize[Path] = 0
endfunction 0
// ****************************************************************************
// ****************************************************************************
//
// ****************************************************************************
// ****************************************************************************
function SearchMapAStar8(Path as integer, SX as integer, SY as integer, TX as integer, TY as integer)
X as integer
Y as integer
XOffset as integer
YOffset as integer
NewCost as integer
if SEARCH_MapInitialized = 0 then exitfunction -1
if SEARCH_PathsInitialized = 0 then exitfunction -1
if Path < 0 or Path > SEARCH_MaxPaths then exitfunction -1
SEARCH_LastSearch = 1
SEARCH_InitializeTileInfo(SX,SY)
while SEARCH_GetLowestCostOpen() > 0
for XOffset = -1 to 1
X = SEARCH_CurrentPosition.X + XOffset
if X >= 0 and X < SEARCH_MapWidth
for YOffset = -1 to 1
if (XOffset || YOffset) <> 0
Y = SEARCH_CurrentPosition.Y + YOffset
if (XOffset || YOffset) = 0
Y = -1
else
` Restrict diagonals
if SEARCH_RestrictedDiagonals
if XOffset <> 0 and YOffset <> 0
if SEARCH_TileInfo[X, SEARCH_CurrentPosition.Y].Status = 2
` This bit depends on Y *not* being set to -1
if SEARCH_TileInfo[SEARCH_CurrentPosition.X, Y].Status = 2
Y = -1
endif
endif
endif
endif
endif
if Y >= 0 and Y < SEARCH_MapHeight
if SEARCH_TileInfo[X,Y].Status = 0
SEARCH_TileInfo[X,Y].ParentX = SEARCH_CurrentPosition.X
SEARCH_TileInfo[X,Y].ParentY = SEARCH_CurrentPosition.Y
if X = TX and Y = TY then exitfunction SEARCH_BuildPath(Path,SX,SY,TX,TY)
SEARCH_TileInfo[X,Y].G = SEARCH_TileInfo[SEARCH_CurrentPosition.X,SEARCH_CurrentPosition.Y].G + SEARCH_SingleStepCost(XOffset,YOffset)
SEARCH_TileInfo[X,Y].H = SEARCH_EstimateDistance(X,Y,TX,TY)
SEARCH_TileInfo[X,Y].F = SEARCH_TileInfo[X,Y].G + SEARCH_TileInfo[X,Y].H
if SEARCH_MaximumCost = 0 or SEARCH_TileInfo[X,Y].G < SEARCH_MaximumCost
SEARCH_AddToOpenList(X,Y)
SEARCH_TileInfo[X,Y].Status = 1
endif
else
if SEARCH_TileInfo[X,Y].Status = 1
NewCost = SEARCH_TileInfo[SEARCH_CurrentPosition.X,SEARCH_CurrentPosition.Y].G + SEARCH_SingleStepCost(XOffset,YOffset)
if SEARCH_TileInfo[X,Y].G > NewCost
SEARCH_TileInfo[X,Y].ParentX = SEARCH_CurrentPosition.X
SEARCH_TileInfo[X,Y].ParentY = SEARCH_CurrentPosition.Y
SEARCH_TileInfo[X,Y].G = NewCost
SEARCH_TileInfo[X,Y].F = NewCost + SEARCH_TileInfo[X,Y].H
SEARCH_AddToOpenList(X,Y)
endif
endif
endif
endif
endif
next YOffset
endif
next XOffset
endwhile
n = SEARCH_ClearPath(Path,SX,SY)
exitfunction n
endfunction 0
// ****************************************************************************
// ****************************************************************************
//
// ****************************************************************************
// ****************************************************************************
function SearchMapAStar4(Path as integer, SX as integer, SY as integer, TX as integer, TY as integer)
X as integer
Y as integer
XOffset as integer
YOffset as integer
NewCost as integer
if SEARCH_MapInitialized = 0 then exitfunction -1
if SEARCH_PathsInitialized = 0 then exitfunction -1
if Path < 0 or Path > SEARCH_MaxPaths then exitfunction -1
SEARCH_LastSearch = 0
SEARCH_InitializeTileInfo(SX,SY)
while SEARCH_GetLowestCostOpen() > 0
for XOffset = -1 to 1
X = SEARCH_CurrentPosition.X + XOffset
if X >= 0 and X < SEARCH_MapWidth
for YOffset = -1 to 1
if (XOffset && YOffset) = 0
Y = SEARCH_CurrentPosition.Y + YOffset
if Y >= 0 and Y < SEARCH_MapHeight
if SEARCH_TileInfo[X,Y].Status = 0
SEARCH_TileInfo[X,Y].ParentX = SEARCH_CurrentPosition.X
SEARCH_TileInfo[X,Y].ParentY = SEARCH_CurrentPosition.Y
if X = TX and Y = TY then exitfunction SEARCH_BuildPath(Path,SX,SY,TX,TY)
SEARCH_TileInfo[X,Y].G = SEARCH_TileInfo[SEARCH_CurrentPosition.X,SEARCH_CurrentPosition.Y].G + 10
SEARCH_TileInfo[X,Y].H = SEARCH_EstimateDistance(X,Y,TX,TY)
SEARCH_TileInfo[X,Y].F = SEARCH_TileInfo[X,Y].G + SEARCH_TileInfo[X,Y].H
if SEARCH_MaximumCost = 0 or Cost < SEARCH_MaximumCost
SEARCH_AddToOpenList(X,Y)
SEARCH_TileInfo[X,Y].Status = 1
endif
else
if SEARCH_TileInfo[X,Y].Status = 1
NewCost = SEARCH_TileInfo[SEARCH_CurrentPosition.X,SEARCH_CurrentPosition.Y].G + 10
if SEARCH_TileInfo[X,Y].G > NewCost
SEARCH_TileInfo[X,Y].ParentX = SEARCH_CurrentPosition.X
SEARCH_TileInfo[X,Y].ParentY = SEARCH_CurrentPosition.Y
SEARCH_TileInfo[X,Y].G = NewCost
SEARCH_TileInfo[X,Y].F = NewCost + SEARCH_TileInfo[X,Y].H
SEARCH_AddToOpenList(X,Y)
endif
endif
endif
endif
endif
next YOffset
endif
next XOffset
endwhile
n = SEARCH_ClearPath(Path,SX,SY)
exitfunction n
endfunction 0
// ****************************************************************************
// ****************************************************************************
//
// ****************************************************************************
// ****************************************************************************
function SearchMapFlood8(Path as integer, SX as integer, SY as integer, TX as integer, TY as integer)
XOffset as integer
YOffset as integer
X as integer
Y as integer
Cost as integer
if SEARCH_MapInitialized = 0 then exitfunction -1
if SEARCH_PathsInitialized = 0 then exitfunction -1
if Path < 0 or Path > SEARCH_MaxPaths then exitfunction -1
if SEARCH_ParametersChanged = 1 or SEARCH_LastSearch <> 3
SEARCH_ParametersChanged = 0
SEARCH_PreviousStart.X = -9999
SEARCH_LastSearch = 3
endif
if SX <> SEARCH_PreviousStart.X or SY <> SEARCH_PreviousStart.Y
SEARCH_PreviousStart.X = SX
SEARCH_PreviousStart.Y = SY
SEARCH_PopulateTileInfo()
SEARCH_TileInfo[SX, SY].Status = 1
SEARCH_TileInfo[SX, SY].ParentX = SX
SEARCH_TileInfo[SX, SY].ParentY = SY
SEARCH_TileInfo[SX, SY].F = 0
SEARCH_AddToOpenList(SX,SY)
SEARCH_SearchMode = 0
while SEARCH_GetLowestCostOpen() > 0
for XOffset = -1 to 1
X = SEARCH_CurrentPosition.X + XOffset
if X >= 0 and X < SEARCH_MapWidth
for YOffset = -1 to 1
Y = SEARCH_CurrentPosition.Y + YOffset
` Not the current cell (offsets both zero)
if (XOffset || YOffset) = 0
Y = -1
else
` Restrict diagonals
if SEARCH_RestrictedDiagonals
if XOffset <> 0 and YOffset <> 0
if SEARCH_TileInfo[X, SEARCH_CurrentPosition.Y].Status = 2
` This bit depends on Y *not* being set to -1
if SEARCH_TileInfo[SEARCH_CurrentPosition.X, Y].Status = 2
Y = -1
endif
endif
endif
endif
endif
if Y >= 0 and Y < SEARCH_MapHeight
if SEARCH_TileInfo[X,Y].Status = 0
SEARCH_TileInfo[X, Y].Status = 1
SEARCH_TileInfo[X, Y].ParentX = SEARCH_CurrentPosition.X
SEARCH_TileInfo[X, Y].ParentY = SEARCH_CurrentPosition.Y
Cost = SEARCH_TileInfo[SEARCH_CurrentPosition.X, SEARCH_CurrentPosition.Y].F + SEARCH_SingleStepCost(XOffset,YOffset)
SEARCH_TileInfo[X, Y].F = Cost
if SEARCH_MaximumCost = 0 or Cost < SEARCH_MaximumCost
SEARCH_AddToOpenList(X,Y)
endif
endif
endif
next YOffset
endif
next XOffset
endwhile
endif
if SEARCH_TileInfo[TX,TY].Status > 0 then exitfunction SEARCH_BuildPath(Path,SX,SY,TX,TY)
n = SEARCH_ClearPath(Path,SX,SY)
exitfunction n
endfunction 0
// ****************************************************************************
// ****************************************************************************
//
// ****************************************************************************
// ****************************************************************************
function SearchMapFlood4(Path as integer, SX as integer, SY as integer, TX as integer, TY as integer)
XOffset as integer
YOffset as integer
X as integer
Y as integer
if SEARCH_MapInitialized = 0 then exitfunction -1
if SEARCH_PathsInitialized = 0 then exitfunction -1
if Path < 0 or Path > SEARCH_MaxPaths then exitfunction -1
if SEARCH_ParametersChanged = 1 or SEARCH_LastSearch <> 2
SEARCH_ParametersChanged = 0
SEARCH_PreviousStart.X = -9999
SEARCH_LastSearch = 2
endif
if SX <> SEARCH_PreviousStart.X or SY <> SEARCH_PreviousStart.Y
SEARCH_PreviousStart.X = SX
SEARCH_PreviousStart.Y = SY
SEARCH_PopulateTileInfo()
SEARCH_OpenListTop=-1
SEARCH_OpenListBottom=-1
SEARCH_TileInfo[SX, SY].Status = 1
SEARCH_TileInfo[SX, SY].ParentX = SX
SEARCH_TileInfo[SX, SY].ParentY = SY
SEARCH_TileInfo[SX, SY].F = 0
SEARCH_QueueOntoOpen(SX, SY)
while SEARCH_UnqueueFromOpen() > 0
for XOffset = -1 to 1
X = SEARCH_CurrentPosition.X + XOffset
if X >= 0 and X < SEARCH_MapWidth
for YOffset = -1 to 1
if (XOffset && YOffset) = 0
Y = SEARCH_CurrentPosition.Y + YOffset
if Y >= 0 and Y < SEARCH_MapHeight
if SEARCH_TileInfo[X, Y].Status = 0
SEARCH_TileInfo[X, Y].Status = 1
SEARCH_TileInfo[X, Y].ParentX = SEARCH_CurrentPosition.X
SEARCH_TileInfo[X, Y].ParentY = SEARCH_CurrentPosition.Y
SEARCH_TileInfo[X, Y].F = SEARCH_TileInfo[SEARCH_CurrentPosition.X, SEARCH_CurrentPosition.Y].F + 10
if SEARCH_MaximumCost = 0 or SEARCH_TileInfo[X, Y].F < SEARCH_MaximumCost
SEARCH_QueueOntoOpen(X, Y)
endif
endif
endif
endif
next YOffset
endif
next XOffset
endwhile
endif
if SEARCH_TileInfo[TX,TY].Status > 0 then exitfunction SEARCH_BuildPath(Path,SX,SY,TX,TY)
n = SEARCH_ClearPath(Path,SX,SY)
exitfunction n
endfunction 0
|