Anders Hesselbom
Programmerare, skeptiker, sekulärhumanist, antirasist.
Författare till bok om C64 och senbliven lantis.
Röstar pirat.
Pathfinding i terräng
2019-01-04
A* (A-star) är en snabb pathfinder-algoritm som kan användas i t.ex. strategispel för att hitta vägar genom labyrinter eller terräng. Christoph Husse publicerade 2010 en briljant implementation i C# som tillåter egna kriterier, definierade i en solver, för kostnaden att flytta sig från en nod till en annan. I följande exempel har jag använt Christophs kod, men jag har skalat bort hans solver och istället hårdkodat in en viktad terräng. Kartan i exemplet är liten, endast 50×25 celler, och kostnaden för att gå på en cell sträcker sig från 0 till 99. De celler som har värdet 100 betraktas som ett oöverstigligt hinder. Bilden visar en körning, där startpositionen är längst ner till vänster.
Det hela utspelar sig i ett vanligt Windows Forms-fönster som har en karta (typen Map) och en stig (typen Path). Både Map och Path presenteras nedan. (De första fyra kodblocken visar alltså innehållet i fönstret, och de resterande kodblocken visar klasserna som används i projektet.)
private Map Map { get; set; }
private Path Path { get; set; }
I fönstrets Load-event skapar jag kartan, kartans terräng och använder sedan klassen som implementerar A* för att hitta den bästa vägen från start (längst ner till vänster) till mål (längst upp till höger). Funktionen FindPath ger null om det inte finns någon väg mellan punkterna.
private void Form1_Load(object sender, EventArgs e)
{
Map = new Map(50, 25);
CreateTerrain(Map);
var aStar = new AStar(Map);
Path = aStar.FindPath(
new Point(0, Map.Height - 1),
new Point(Map.Width - 1, 0))
?? new Path();
Invalidate();
}
Funktionen för att generera terrängen, CreateTerrain, skapar ett godtyckligt bergslandskap.
private void CreateTerrain(Map map)
{
var rnd = new Random();
var walkcosts = new int[map.Width + 2, map.Height + 2];
for (var y = 0; y < map.Height + 2; y++)
for (var x = 0; x < map.Width + 2; x++)
if (rnd.Next(0, 3) == 1)
walkcosts[x, y] = rnd.Next(-10, 300);
int GetAverage(int[,] a, int x, int y) =>
(a[x - 1, y - 1] + a[x, y - 1] + a[x + 1, y - 1]
+ a[x - 1, y] + a[x, y] + a[x + 1, y]
+ a[x - 1, y + 1] + a[x, y + 1] + a[x + 1, y + 1]) / 9;
for (var y = 0; y < map.Height; y++)
{
for (var x = 0; x < map.Width; x++)
{
var avg = GetAverage(walkcosts, x + 1, y + 1);
if (avg < 0)
avg = 0;
else if (avg > 100)
avg = 100;
map.SetWalkCost(x, y, (byte)avg);
}
}
}
I fönstrets Paint-event ritas det hela ut på skärmen.
private void Form1_Paint(object sender, PaintEventArgs e)
{
e.Graphics.Clear(Color.Black);
var m = new MapRenderer(Map, Path, 24);
m.Draw(e.Graphics, 0, 0);
}
Klassen MapRenderer som ansvarar för ritandet ser ut så här:
using System.Drawing;
using System.Drawing.Drawing2D;
namespace PathFinderAStar
{
public class MapRenderer
{
public Map Map { get; }
public Path Path { get; }
public int CellSize { get; }
public MapRenderer(Map map, Path path, int cellSize)
{
Map = map;
Path = path;
CellSize = cellSize;
}
public void Draw(Graphics g, int offsetX, int offsetY)
{
var xpos = offsetX;
var ypos = offsetY;
using (var font = new Font(FontFamily.GenericSansSerif, 8))
{
for (var y = 0; y < Map.Height; y++)
{
for (var x = 0; x < Map.Width; x++)
{
g.SmoothingMode = SmoothingMode.None;
g.DrawRectangle(
Pens.Gray,
xpos,
ypos,
CellSize,
CellSize);
using (var b = new SolidBrush(
Color.FromArgb(Map.GetWalkCost(x, y) * 2,
0,
0)))
g.FillRectangle(
b,
xpos + 2,
ypos + 2,
CellSize - 3,
CellSize - 3);
var pathCell = Path.GetCellAt(x, y);
g.SmoothingMode = SmoothingMode.HighQuality;
if (pathCell != null)
g.FillEllipse(
Brushes.CadetBlue,
xpos + 5,
ypos + 5,
CellSize - 9,
CellSize - 9);
g.DrawString(
Map.GetWalkCost(x, y).ToString(),
font,
Brushes.White,
xpos + 1,
ypos + 1);
xpos += CellSize;
}
xpos = offsetX;
ypos += CellSize;
}
}
}
}
}
Klassen Map beskriver en 2-dimensionell karta, på vilken vi ska hitta en stig.
using System;
using System.Diagnostics;
using System.Drawing;
namespace PathFinderAStar
{
public class Map
{
private readonly MapCell[,] _terrain;
public int Width { get; }
public int Height { get; }
public Map(int width, int height)
{
if (width < 2 || width > 2000)
throw new ArgumentOutOfRangeException(nameof(width));
if (height < 2 || height > 2000)
throw new ArgumentOutOfRangeException(nameof(height));
Width = width;
Height = height;
_terrain = new MapCell[width, height];
}
public byte GetWalkCost(int x, int y)
{
if (x < 0 || x >= Width)
throw new ArgumentOutOfRangeException(nameof(x));
if (y < 0 || y >= Height)
throw new ArgumentOutOfRangeException(nameof(y));
var cell = _terrain[x, y];
if (cell == null)
return 0;
Debug.Assert(cell.X == x);
Debug.Assert(cell.Y == y);
return cell.WalkCost;
}
public void SetWalkCost(int x, int y, byte walkCost)
{
if (x < 0 || x >= Width)
throw new ArgumentOutOfRangeException(nameof(x));
if (y < 0 || y >= Height)
throw new ArgumentOutOfRangeException(nameof(y));
if (walkCost > 100)
throw new ArgumentOutOfRangeException(nameof(walkCost));
var cell = _terrain[x, y];
if (cell == null)
{
cell = new MapCell(x, y, walkCost);
_terrain[x, y] = cell;
return;
}
Debug.Assert(cell.X == x);
Debug.Assert(cell.Y == y);
cell.WalkCost = walkCost;
}
public void Clear()
{
for (var y = 0; y < Height; y++)
for (var x = 0; x < Width; x++)
_terrain[x, y] = null;
}
public MapCell GetCell(int x, int y) =>
GetCell(new Point(x, y));
public MapCell GetCell(Point p)
{
if (p.X < 0 || p.X >= Width || p.Y < 0 || p.Y >= Height)
return null;
var c = _terrain[p.X, p.Y];
return c;
}
public double GetDistance(MapCell start, MapCell goal)
{
var actualDistance = Math.Sqrt((start.X - goal.X)
* (start.X - goal.X)
+ (start.Y - goal.Y)
* (start.Y - goal.Y));
return actualDistance + (double)goal.WalkCost / 10;
}
public void Add(MapCell cell)
{
if (cell == null)
throw new ArgumentNullException(nameof(cell));
if (cell.X < 0 || cell.X >= Width)
throw new ArgumentOutOfRangeException(nameof(cell.X));
if (cell.Y < 0 || cell.Y >= Height)
throw new ArgumentOutOfRangeException(nameof(cell.Y));
if (_terrain[cell.X, cell.Y] != null)
throw new SystemException();
_terrain[cell.X, cell.Y] = cell;
}
public void Remove(MapCell cell)
{
if (cell == null)
throw new ArgumentNullException(nameof(cell));
if (_terrain[cell.X, cell.Y] == null)
throw new SystemException();
if (_terrain[cell.X, cell.Y] != cell)
throw new SystemException();
_terrain[cell.X, cell.Y] = null;
}
public bool Contains(MapCell cell) =>
_terrain[cell.X, cell.Y] != null;
public void SetWithHistory(int x, int y, MapCell cell) =>
_terrain[x, y] = cell;
public bool IsEmpty
{
get
{
for (var y = 0; y < Height; y++)
for (var x = 0; x < Width; x++)
if (_terrain[x, y] != null)
return false;
return true;
}
}
public int CompareCells(MapCell c1, MapCell c2)
{
if (c1 == null)
throw new ArgumentNullException(nameof(c1));
if (c2 == null)
throw new ArgumentNullException(nameof(c2));
if (c1.F < c2.F)
return -1;
if (c1.F > c2.F)
return 1;
return 0;
}
}
}
Stigen beskrivs av klassen Path:
using System.Collections.Generic;
using System.Linq;
namespace PathFinderAStar
{
public class Path : List<MapCell>
{
public Path()
{
}
public Path(int count)
{
for (var i = 0; i < count; i++)
Add(null);
}
public MapCell GetCellAt(int x, int y) =>
this.FirstOrDefault(c => c.X == x && c.Y == y);
}
}
Både kartan (Map) och stigen (Path) består av referenser till celler som beskriver hur terrängen ser ut på en specifik plats. Typen heter MapCell (G, H, F och Index används av A*.):
namespace PathFinderAStar
{
public class MapCell
{
private byte _walkCost;
public int X { get; }
public int Y { get; }
public double G { get; set; }
public double H { get; set; }
public double F { get; set; }
public int Index { get; set; }
public byte WalkCost
{
get => _walkCost;
set => _walkCost = value > 100 ? (byte)100 : value;
}
public bool IsWall =>
WalkCost >= 100;
public MapCell(int x, int y, byte walkCost)
{
X = x;
Y = y;
WalkCost = walkCost;
}
}
}
Sen har vi själva A*-algoritmen, såsom den implementerats av Christoph Husse, med mina förenklingar. Den konsumeras direkt i formulärets Load-event och ligger i klassen AStar.
using System.Drawing;
namespace PathFinderAStar
{
public class AStar
{
private Map Map { get; }
private Map ClosedSet { get; }
private Map OpenSet { get; }
private Map CameFrom { get; }
private Map RuntimeGrid { get; }
private PriorityQueue OrderedOpenSet { get; }
public AStar(Map map)
{
Map = map;
ClosedSet = new Map(Map.Width, Map.Height);
OpenSet = new Map(Map.Width, Map.Height);
CameFrom = new Map(Map.Width, Map.Height);
RuntimeGrid = new Map(Map.Width, Map.Height);
OrderedOpenSet = new PriorityQueue(Map);
}
public Path FindPath(Point start, Point goal)
{
var startCell = Map.GetCell(start);
var goalCell = Map.GetCell(goal);
if (startCell == goalCell)
return new Path { startCell };
ClosedSet.Clear();
OpenSet.Clear();
CameFrom.Clear();
RuntimeGrid.Clear();
OrderedOpenSet.Clear();
startCell.G = 0.0;
startCell.H = Map.GetDistance(startCell, goalCell);
startCell.F = startCell.H;
OpenSet.Add(startCell);
OrderedOpenSet.Push(startCell);
RuntimeGrid.Add(startCell);
var neighbours = new Path(8);
while (!OpenSet.IsEmpty)
{
var next = OrderedOpenSet.Pop();
if (next == goalCell)
{
var result = ReconstructPath(
CameFrom,
CameFrom.GetCell(goalCell.X, goalCell.Y));
result.Add(goalCell);
return result;
}
OpenSet.Remove(next);
ClosedSet.Add(next);
StoreNeighbours(next, neighbours);
foreach (var currentNeighbour in neighbours)
{
if (currentNeighbour == null
|| currentNeighbour.IsWall
|| ClosedSet.Contains(currentNeighbour))
continue;
var tentativeG = RuntimeGrid.GetCell(next.X, next.Y).G
+ Map.GetDistance(next, currentNeighbour);
if (!TentativeIsBetter(currentNeighbour,
tentativeG, out var added))
continue;
CameFrom.SetWithHistory(
currentNeighbour.X,
currentNeighbour.Y,
next);
if (!RuntimeGrid.Contains(currentNeighbour))
RuntimeGrid.Add(currentNeighbour);
currentNeighbour.G = tentativeG;
currentNeighbour.H = Map.GetDistance(
currentNeighbour,
goalCell);
currentNeighbour.F = currentNeighbour.G
+ currentNeighbour.H;
if (added)
OrderedOpenSet.Push(currentNeighbour);
else
OrderedOpenSet.Update(currentNeighbour);
}
}
return null;
}
private bool TentativeIsBetter(
MapCell currentNeighbour,
double tentativeG,
out bool added)
{
if (!OpenSet.Contains(currentNeighbour))
{
OpenSet.Add(currentNeighbour);
added = true;
return true;
}
added = false;
return tentativeG < RuntimeGrid.GetCell(
currentNeighbour.X,
currentNeighbour.Y).G;
}
private void StoreNeighbours(MapCell c, Path neighbours)
{
neighbours[0] = Map.GetCell(c.X - 1, c.Y - 1);
neighbours[1] = Map.GetCell(c.X, c.Y - 1);
neighbours[2] = Map.GetCell(c.X + 1, c.Y - 1);
neighbours[3] = Map.GetCell(c.X - 1, c.Y);
neighbours[4] = Map.GetCell(c.X + 1, c.Y);
neighbours[5] = Map.GetCell(c.X - 1, c.Y + 1);
neighbours[6] = Map.GetCell(c.X, c.Y + 1);
neighbours[7] = Map.GetCell(c.X + 1, c.Y + 1);
}
private Path ReconstructPath(Map cameFrom, MapCell currentCell)
{
void ReconstructPathRecursive(
Map recursiveCameFrom,
MapCell recursiveCurrentCell,
Path result)
{
var item = recursiveCameFrom.GetCell(
recursiveCurrentCell.X,
recursiveCurrentCell.Y);
if (item != null)
ReconstructPathRecursive(
recursiveCameFrom,
item,
result);
result.Add(recursiveCurrentCell);
}
var path = new Path();
ReconstructPathRecursive(cameFrom, currentCell, path);
return path;
}
}
}
Och slutligen, typen PriorityQueue som algoritmen använder för att hålla reda på vilka celler som undersöks, ser ut så här:
using System.Collections.Generic;
namespace PathFinderAStar
{
public class PriorityQueue
{
private List<MapCell> InnerList { get; } = new List<MapCell>();
private Map Map { get; }
public PriorityQueue(Map map)
{
Map = map;
}
public void Clear() =>
InnerList.Clear();
public int Push(MapCell c)
{
var index1 = InnerList.Count;
c.Index = index1;
InnerList.Add(c);
do
{
if (index1 <= 0)
break;
var index2 = (index1 - 1) / 2;
if (Compare(index1, index2) < 0)
{
SwitchElements(index1, index2);
index1 = index2;
continue;
}
break;
} while (true);
return index1;
}
public MapCell Pop()
{
var result = InnerList[0];
InnerList[0] = InnerList[InnerList.Count - 1];
InnerList[0].Index = 0;
InnerList.RemoveAt(InnerList.Count - 1);
result.Index = -1;
var p = 0;
do
{
var pn = p;
var p1 = 2 * p + 1;
var p2 = 2 * p + 2;
if (InnerList.Count > p1 && Compare(p, p1) > 0)
p = p1;
if (InnerList.Count > p2 && Compare(p, p2) > 0)
p = p2;
if (p == pn)
break;
SwitchElements(p, pn);
} while (true);
return result;
}
public void Update(MapCell cell)
{
var count = InnerList.Count;
while (cell.Index - 1 > 0
&& Compare(cell.Index - 1, cell.Index) > 0)
SwitchElements(cell.Index - 1, cell.Index);
while (cell.Index + 1 < count
&& Compare(cell.Index + 1, cell.Index) < 0)
SwitchElements(cell.Index + 1, cell.Index);
}
private int Compare(int i1, int i2) =>
Map.CompareCells(InnerList[i1], InnerList[i2]);
private void SwitchElements(int i1, int i2)
{
var c1 = InnerList[i1];
InnerList[i1] = InnerList[i2];
InnerList[i2] = c1;
InnerList[i1].Index = i1;
InnerList[i2].Index = i2;
}
}
}
Med denna kod på plats har man en enkel funktion (FindPath) att anropa för att hitta en stig mellan två punkter (om sådan finns). Och om man vill anpassa kriterierna för hur en stig väljs, t.ex. hur terrängen ska viktas mot avståndet, görs det i funktionen GetDistance i klassen Map.
Categories: C#
En kopp kaffe!
Bjud mig på en kopp kaffe (20:-) som tack för bra innehåll!
Följ mig
|
|
|
|
Leave a Reply