Code archives/Algorithms/A* (astar) routine
This code has been declared by its author to be Public Domain code.
Download source code
| |||||
BlitzMax port of Patrick Lester's "A* Pathfinding for Beginners" http://www.policyalmanac.org/games/aStarTutorial.htm | |||||
'=========================================================================================================================== '=========================================================================================================================== ' CREDITS ' ======= ' BlitzMax port of Patrick Lester's "A* Pathfinding for Beginners" http://www.policyalmanac.org/games/aStarTutorial.htm ' Conversion by 'CoffeeDotBean' http://www.blitzbasic.com/Account/showuser.php?id=584 (June 26 2016) ' ' ' HOW TO USE ' ========== '1. Include this file (.bmx) at the top of your program. ' '2. Create a NEW TPath_Astar:Object. e.g myPath:TPath_Astar = New TPath_Astar; myPath is your hook going forward. ' '3. Call Prepare() Method and pass in the required information. ' Arr[,] = 2d Int Array containing your Map data, does not need to be specially formatted. ' UnWalkableValues[] = Int Array containing Values that the Search will consider as Walls & impassable terrain. ' StartX/Y = Start Grid ref. ' TargetX/Y = Target Grid ref. ' '4. Call Search() Method to look for a Path, call PathFound() to know if a path was found ' EightWay = If False diaganls will be ignored. ' CutCorners = (only applicanle with Eightway=True), Cut diagonaly through corners. ' '5. Return A found Path, all Methods will return (-1) if no path is avaliable ' GetPathInt() = Return Int Array with directions (Start ==> Target) 1,2,3,4,5,6,7,8 (1=up, 2=up&right, 3=right, 4=down&right etc..). ' GetPathStr() = Return String Array with compass directions (Start ==> Target) N,NE,E,SE,S,SW,W,NW. ' GetPathRef() = Return Int Array with Array grid references (probably not useful). ' ' ' OTHER METHODS ' ============= '*. PathFound() = Return True if a path was found after calling Search() Method. '*. PathClear() = Flag the path as not found (does not nullify any other data). '*. GetPathLength() = Return Length of the path as steps (tiles) between Start and Target along the path. '*. GetPathDistance() = Return Distance between Start and Target in Pixels along the path. '=========================================================================================================================== '=========================================================================================================================== Type TPath_Astar Const NOTFINISHED:Int = 0 Const NOTSTARTED:Int = 0 Const FOUND:Int = 1 Const NONEXISTENT:Int = 2 Const WALKABLE:Int = 0 Const UNWALKABLE:Int = 1 Const ONCLOSEDLIST:Int = 2 Const ONOPENLIST:Int = 1 Field MapWidth:Int Field MapHeight:Int Field Walkability:Int[,] Field OpenList:Int[] Field WhichList:Int[,] Field OpenX:Int[] Field OpenY:Int[] Field ParentX:Int[,] Field ParentY:Int[,] Field FCost:Int[] Field GCost:Int[,] Field HCost:Int[] Field StartX:Int Field StartY:Int Field TargetX:Int Field TargetY:Int Field NumberOfOpenListItems:Int Field NewOpenListItemID:Int Field Path:Int Field Steps:TList rem bbdoc: Flag path as not exisiting end rem Method Clear:Int() Path = NONEXISTENT End Method rem bbdoc: Search for a path, EightWay=False means no diagonals, CutCorners only applicable with diagonals (Eightway) end rem Method Search:Int(EightWay:Int = True, CutCorners:Int = False) NumberOfOpenListItems = 1 OpenList[1] = 1 OpenX[1] = StartX OpenY[1] = StartY Repeat '6 ------------------------------------------------------------------- If NumberOfOpenListItems <> 0 Local ParentXval:Int = OpenX[OpenList[1]] Local ParentYval:Int = OpenY[OpenList[1]] WhichList[ParentXval, ParentYval] = ONCLOSEDLIST NumberOfOpenListItems:-1 OpenList[1] = OpenList[NumberOfOpenListItems + 1] Local v:Int = 1 Repeat Local u:Int = v If 2 * u + 1 <= NumberOfOpenListItems If FCost[OpenList[u]] >= FCost[OpenList[2 * u]] Then v = 2 * u If FCost[OpenList[v]] >= FCost[OpenList[2 * u + 1]] Then v = 2 * u + 1 Else If 2 * u <= NumberOfOpenListItems If FCost[OpenList[u]] >= FCost[OpenList[2 * u]] Then v = 2 * u End If End If If u <> v Local temp:Int = OpenList[u] OpenList[u] = OpenList[v] OpenList[v] = temp Else Exit End If Forever ' 7 ------------------------------------------------------------------- For Local b:Int = ParentYval - 1 To ParentYval + 1 For Local a:Int = ParentXval - 1 To ParentXval + 1 If Not EightWay If a = ParentXval + 1 And b = ParentYval + 1 Then Continue If a = ParentXval - 1 And b = ParentYval - 1 Then Continue If a = ParentXval + 1 And b = ParentYval - 1 Then Continue If a = ParentXval - 1 And b = ParentYval + 1 Then Continue EndIf If a <> - 1 And b <> - 1 And a <> MapWidth And b <> MapHeight If whichList[a, b] <> ONCLOSEDLIST If walkability[a, b] <> unwalkable Local Corner:Int = WALKABLE If CutCorners = False If a = ParentXval - 1 If b = ParentYval - 1 If Walkability[ParentXval - 1, ParentYval] = UNWALKABLE Or Walkability[ParentXval, ParentYval - 1] = UNWALKABLE Then Corner = UNWALKABLE Else If b = ParentYval + 1 If Walkability[ParentXval, ParentYval + 1] = UNWALKABLE Or Walkability[ParentXval - 1, ParentYval] = UNWALKABLE Then Corner = UNWALKABLE End If Else If a = ParentXval + 1 If b = ParentYval - 1 If Walkability[ParentXval, ParentYval - 1] = UNWALKABLE Or Walkability[ParentXval + 1, ParentYval] = UNWALKABLE Then Corner = UNWALKABLE Else If b = ParentYval + 1 If Walkability[ParentXval + 1, ParentYval] = UNWALKABLE Or Walkability[ParentXval, ParentYval + 1] = UNWALKABLE Then Corner = UNWALKABLE End If End If EndIf If Corner = WALKABLE If whichList[a, b] <> ONOPENLIST NewOpenListItemID:+1 Local m:Int = NumberOfOpenListItems + 1 OpenList[m] = NewOpenListItemID OpenX[NewOpenListItemID] = a openY[NewOpenListItemID] = b If Abs(a - ParentXval) = 1 And Abs(b - ParentYval) = 1 Gcost[a, b] = Gcost[ParentXval, ParentYval] + 14 Else Gcost[a, b] = Gcost[ParentXval, ParentYval] + 10 End If HCost[OpenList[m]] = EstimateHcost(a, b) FCost[OpenList[m]] = GCost[a, b] + HCost[openList[m]] ParentX[a, b] = ParentXval ParentY[a, b] = ParentYval While m <> 1 If FCost[openList[m]] <= FCost[OpenList[m / 2]] Local temp:Int = OpenList[m / 2] OpenList[m / 2] = OpenList[m] OpenList[m] = temp m = m / 2 Else Exit End If Wend NumberOfOpenListItems:+1 WhichList[a, b] = ONOPENLIST ' 8 ------------------------------------------------------------------- Else 'whichList[a, b] <> ONOPENLIST Local tempGcost:Int If Abs(a - ParentXval) = 1 And Abs(b - ParentYval) = 1 tempGcost = Gcost[ParentXval, ParentYval] + 14 Else tempGcost = Gcost[ParentXval, ParentYval] + 10 End If If tempGcost < GCost[a, b] ParentX[a, b] = ParentXval ParentY[a, b] = ParentYval GCost[a, b] = tempGcost For Local x:Int = 1 To NumberOfOpenListItems If OpenX[OpenList[x]] = a And OpenY[OpenList[x]] = b FCost[OpenList[x]] = Gcost[a, b] + HCost[OpenList[x]] Local m:Int = x While m <> 1 If FCost[OpenList[m]] < FCost[OpenList[m / 2]] Then Local temp:Int = OpenList[m / 2] OpenList[m / 2] = openList[m] OpenList[m] = temp m = m/2 Else Exit 'while/wend End If Wend Exit 'for x = loop End If 'If openX(openList(x)) = a Next 'For x = 1 To numberOfOpenListItems End If 'If tempGcost < Gcost(a,b) End If ' If not already on the open list End If ' If corner = walkable End If ' If not a wall/obstacle cell End If 'If not already on the closed list End If 'If not off the map. Next Next ' 9 ------------------------------------------------------------------- Else 'NumberOfOpenListItems / If open list is empty then there is no path. Path = NONEXISTENT Exit EndIf 'NumberOfOpenListItems If WhichList[TargetX, TargetY] = ONOPENLIST Path = FOUND Exit EndIf Forever ' 10 ------------------------------------------------------------------- If Path = FOUND Local pathX:Int = TargetX Local pathY:Int = TargetY Steps.AddFirst(String(pathX + (pathY * MapWidth))) Repeat Local tempx:Int = ParentX[pathX, pathY] pathY = ParentY[pathX, pathY] pathX = tempx Steps.AddFirst(String(pathX + (pathY * MapWidth))) Until pathX = StartX And pathY = StartY End If End Method rem bbdoc: Prepare to do a search, ALWAYS call before calling Search() Method end rem Method Prepare:Int(Arr:Int[,], UnWalkableValues:Int[], StartX:Int, StartY:Int, TargetX:Int, TargetY:Int) 'Store start and target locations Self.StartX = StartX Self.StartY = StartY Self.TargetX = TargetX Self.TargetY = TargetY 'Initalise arrays MapWidth = arr.Dimensions()[0] MapHeight = arr.Dimensions()[1] Walkability = New Int[MapWidth, MapHeight] WhichList = New Int[MapWidth, MapHeight] ParentX = New Int[MapWidth, MapHeight] ParentY = New Int[MapWidth, MapHeight] GCost = New Int[MapWidth, MapHeight] OpenList = New Int[MapWidth * MapHeight + 2] OpenX = New Int[MapWidth * MapHeight + 2] OpenY = New Int[MapWidth * MapHeight + 2] FCost = New Int[MapWidth * MapHeight + 2] HCost = New Int[MapWidth * MapHeight + 2] 'Reset Fields NumberOfOpenListItems = 0 NewOpenListItemID = 0 Steps = New TList 'Make array to check walkable squares For Local x:Int = 0 Until MapWidth For Local y:Int = 0 Until MapHeight For Local a:Int = 0 Until UnWalkableValues.Length If Arr[x, y] = UnWalkableValues[a] Then Walkability[x, y] = UNWALKABLE Next Next Next End Method rem bbdoc: Return Estimated cost (H)euristic (used in search Method), three methods available, default is Manhatten end rem Method EstimateHcost:Int(a:Int, b:Int, Formula:Int = 0) Local h:Int Select Formula Case 0 'Manhattan (dx+dy) (DEFAULT) h = 10 * (Abs(a - TargetX) + Abs(b - TargetY)) Case 1 'Diagonal Shortcut Estimation Method (NOT USED BY DEFAULT) Local xDistance:Int = Abs(a - TargetX) Local yDistance:Int = Abs(b - TargetY) If xDistance > yDistance Then h = 14 * yDistance + 10 * (xDistance - yDistance) Else h = 14 * xDistance + 10 * (yDistance - xDistance) End If Case 2 'Max(dx,dy) (NOT USED BY DEFAULT) Local xDistance:Int = Abs(a - TargetX) Local yDistance:Int = Abs(b - TargetY) If xDistance > yDistance Then H = 10*xDistance Else H = 10*yDistance End If End Select Return h End Method rem bbdoc: DEBUG - draw path results (wouldn't typicaly use this, but useful for testing/debugging) end rem Method Render:Int(TileSize:Int, JustShowPath:Int = True) If Path <> FOUND Return 0 If Not JustShowPath 'List type, green=openlist, red=closedlist For Local x:Int = 0 Until MapWidth For Local y:Int = 0 Until MapHeight If WhichList[x, y] = ONOPENLIST SetColor 106, 193, 1 ElseIf WhichList[x, y] = ONCLOSEDLIST SetColor 195, 15, 1 Else Continue EndIf If (x = StartX And y = StartY) Or (x = TargetX And y = TargetY) SetColor 42, 155, 196 DrawRect((x * TileSize), (y * TileSize), TileSize, TileSize) Else DrawRect((x * TileSize), (y * TileSize), TileSize, TileSize) End If Next Next 'Parent pointers SetColor 0, 0, 0 For Local x:Int = 0 Until MapWidth For Local y:Int = 0 Until MapHeight If WhichList[x, y] = 0 Continue If x = StartX And y = StartY Continue Local index:Int = (x + (y * MapWidth)) Local val:Int = (ParentX[x, y] + (ParentY[x, y] * MapWidth)) Local angle:Int Select val - index Case 1 ;angle = 90 Case - 1;angle = 270 Case MapWidth;angle = 180 Case - MapWidth;angle = 0 Case MapWidth + 1;angle = 135 Case MapWidth - 1;angle = 225 Case - MapWidth - 1;angle = 315 Case - MapWidth + 1;angle = 45 End Select DrawOval(((x + 0.5) * TileSize) - ((TileSize / 5) / 2), .. ((y + 0.5) * TileSize) - ((TileSize / 5) / 2), .. (TileSize / 5), (TileSize / 5)) DrawLine((x + 0.5) * TileSize, (y + 0.5) * TileSize, .. ((x + 0.5) * TileSize) + (Sin(180 - angle) * (TileSize / 3)), .. ((y + 0.5) * TileSize) + (Cos(180-angle) * (TileSize/3))) Next Next 'Costs SetColor 0, 0, 0 For Local x:Int = 0 Until MapWidth For Local y:Int = 0 Until MapHeight If WhichList[x, y] = 0 Continue If x = StartX And y = StartY Continue DrawText(GCost[x, y] + EstimateHcost(x, y), (x * TileSize) + 2, (y * TileSize) + 2) DrawText(GCost[x, y], (x * TileSize) + 2, ((y + 1) * TileSize) - TextHeight("")) DrawText(EstimateHcost(x, y), ((x + 1) * TileSize) - TextWidth(String EstimateHcost(x, y)) - 4, ((y + 1) * TileSize) - TextHeight("")) Next Next SetColor 255, 255, 255 EndIf 'Path If Path <> FOUND Then Return 0 Local b:Int[] = GetPathRef() For Local x:Int = 0 Until MapWidth For Local y:Int = 0 Until MapHeight For Local a:Int = 0 Until b.Length If b[a] = (x + (y * MapWidth)) DrawOval(((x + 0.5) * TileSize)-(TileSize / 8), ((y + 0.5) * TileSize)-(TileSize / 8), (TileSize / 4), (TileSize / 4)) Local angle:Int Local lineLen:Int = TileSize If a = b.Length - 1 Continue Select b[a + 1] - b[a] Case 1 ;angle = 90 Case - 1;angle = 270 Case MapWidth;angle = 180 Case - MapWidth;angle = 0 Case MapWidth + 1;angle = 135 ; lineLen:+(TileSize / 3) Case MapWidth - 1;angle = 225 ; lineLen:+(TileSize / 3) Case - MapWidth - 1;angle = 315; lineLen:+(TileSize / 3) Case - MapWidth + 1;angle = 45 ; lineLen:+(TileSize / 3) End Select DrawLine((x + 0.5) * TileSize, (y + 0.5) * TileSize, .. ((x + 0.5) * TileSize) + (Sin(180 - angle) * (lineLen)), .. ((y + 0.5) * TileSize) + (Cos(180 - angle) * (lineLen))) EndIf Next Next Next End Method rem bbdoc: Return a FOUND path as INT[] Array (Start --> Target) representing directions CLOCKWISE; (1)=up, (2)=up&right, (3)=right, (4)=down&right .. etc .. end rem Method GetPathInt:Int[] () If Path <> FOUND Return[- 1] Local b:Int[Steps.Count() - 1] For Local i:Int = 0 Until b.Length Local a:Int = Int(String Steps.ValueAtIndex(i)) Local c:Int = Int(String Steps.ValueAtIndex(i + 1)) Select a - c Case 1 ;b[i] = 7 Case - 1;b[i] = 3 Case MapWidth;b[i] = 1 Case - MapWidth;b[i] = 5 Case MapWidth + 1;b[i] = 8 Case MapWidth - 1;b[i] = 2 Case - MapWidth - 1; b[i] = 4 Case - MapWidth + 1;b[i] = 6 End Select Next Return b End Method rem bbdoc: Return a FOUND path as STRING[] Array (Start --> Target) representing compass directions CLOCKWISE; "N", "NE", "E", "SE", "S", "SW", "W", "NW" end rem Method GetPathStr:String[] () If Path <> FOUND Return["NO PATH"] Local a:Int[] = GetPathInt() Local b:String[a.Length] For Local i:Int = 0 Until a.Length Select a[i] Case 1;b[i] = "N" Case 2;b[i] = "NE" Case 3;b[i] = "E" Case 4;b[i] = "SE" Case 5;b[i] = "S" Case 6;b[i] = "SW" Case 7;b[i] = "W" Case 8;b[i] = "NW" End Select Next Return b End Method rem bbdoc: Return a FOUND path as Array (x,y) Tile reference values (probably not that useful) end rem Method GetPathRef:Int[] () If Path <> FOUND Return[- 1] Local b:Int[GetPathInt().Length + 1] Local pathX:Int = TargetX Local pathY:Int = TargetY Local i:Int = b.Length - 1 b[i] = (TargetX + (TargetY * MapWidth)) ; i:-1 Repeat Local tempx:Int = ParentX[pathX, pathY] pathY = ParentY[pathX, pathY] pathX = tempx b[i] = (pathX + (pathY * MapWidth)) i:-1 Until pathX = StartX And pathY = StartY Return b End Method rem bbdoc: Return how many steps are in the FOUND path end rem Method GetPathLength:Int() If Path <> FOUND Return - 1 Return Steps.Count() End Method rem bbdoc: Return the length (in pixels) of the FOUND path (going through the center of each tile) end rem Method GetPathDistance:Int(DiagonalCost:Int = 14, VertHorzCost:Int = 10) If Path <> FOUND Return - 1 Local a:Int[] = GetPathInt() Local cost:Int For Local i:Int = 0 Until a.Length Select a[i] Case 1;cost:+VertHorzCost Case 2;cost:+DiagonalCost Case 3;cost:+VertHorzCost Case 4;cost:+DiagonalCost Case 5;cost:+VertHorzCost Case 6;cost:+DiagonalCost Case 7;cost:+VertHorzCost Case 8;cost:+DiagonalCost End Select Next Return cost End Method rem bbdoc: Return True if a path has been found after the last Search() call end rem Method PathFound:Int() If Path = FOUND Then Return 1 End Method End Type |
Comments
| ||
Download this and a demo.bmx from my google drive https://drive.google.com/open?id=0B1jBbicq8v_dWk5FRVlWWVZnRFE |
Code Archives Forum