A* 寻路算法简单实现
程序员文章站
2024-03-16 22:21:34
...
基于c# framwork,自带演示界面,原理略过,只实现了最简单的功能,由于文件代码较多,只贴了关键部分,整理即可运行。
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
namespace AStartTest
{
public class SearchGrid
{
private Form1 mForm; // 因为form里定义了起始点等信息,所以这里需要保存一个引用,方便访问
private SearchGrid mParent;
public int X;
public int Y; // 格子坐标
public SearchGrid(Form1 form, int x, int y, SearchGrid grid = null)
{
mForm = form;
X = x;
Y = y;
SetParent(grid);
}
public float G()
{
return mParent == null ? 0 : mParent.G() + 1;
}
public float H()
{
return Math.Abs(X - mForm.EndX) + Math.Abs(Y - mForm.EndY);
}
public float F()
{
return G() + H();
}
public void SetParent(SearchGrid g)
{
mParent = g;
}
public SearchGrid GetParent()
{
return mParent;
}
public bool Same(SearchGrid g)
{
return Same(g.X, g.Y);
}
public bool Same(int x, int y)
{
return x == X && y == Y;
}
public void PaintGrid(Graphics g, Brush fillBrush, Brush textBrush, Font textFont)
{
var fb = fillBrush;
if (Same((int)mForm.BeginX, (int)mForm.BeginY))
{
fb = new SolidBrush(Color.Green);
}
else if (Same((int)mForm.EndX, (int)mForm.EndY))
{
fb = new SolidBrush(Color.Red);
}
g.FillRectangle(fb, mForm.StartX + X * mForm.GridW + 2, mForm.StartY + Y * mForm.GridW + 2, mForm.GridW -3, mForm.GridW - 3 );
if (textBrush != null && textFont != null)
{
g.DrawString(F().ToString(), textFont, textBrush, mForm.StartX + X * mForm.GridW + 3, mForm.StartY + Y * mForm.GridW + 3);
g.DrawString(G().ToString(), textFont, textBrush, mForm.StartX + X * mForm.GridW + 3, mForm.StartY + (Y + 1) * mForm.GridW - 18);
g.DrawString(H().ToString(), textFont, textBrush, mForm.StartX + (X + 1) * mForm.GridW - 18, mForm.StartY + (Y + 1) * mForm.GridW - 18);
}
}
}
public partial class Form1 : Form
{
// 网格阻挡信息矩阵 0为通达,大于0为阻挡
private int[][] mGridData = new int[][] {
new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,5,5,0,5,0,0,0,},
new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,0,0,0,5,0,5,0,},
new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,5,0,5,0,5,0,},
new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,0,5,0,0,0,5,0,},
new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,0,0,0,5,0,5,0,},
new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,5,5,5,5,0,5,0,},
new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,0,0,5,0,0,5,0,},
new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,0,0,5,5,5,5,0,},
new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,},
new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,5,5,5,5,5,5,0,},
new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,},
new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,0,0,0,0,0,0,0,},
new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,0,0,0,0,0,0,0,},
new int[] { 5,5,5,5,5,0,0,0,0,0,0,0,5,0,0,0,0,0,0,0,},
new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,0,0,0,0,0,0,0,},
new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,0,0,0,0,0,0,0,},
new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,0,0,0,0,0,0,0,},
new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,0,0,0,0,0,0,0,},
new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,0,0,0,0,0,0,0,},
new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,0,0,0,0,0,0,0,},
};
public float StartX = 20f;
public float StartY = 20f;
public float GridW = 45f;
// 起始点对于格子的坐标
public float BeginX = 1f, BeginY = 1f;
public float EndX = 18f, EndY = 19f;
public Form1()
{
InitializeComponent();
}
/// <summary>
/// 【按钮点击事件】,绘制格子基本信息
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void button1_Click(object sender, EventArgs e)
{
Graphics g = this.CreateGraphics();
Pen pen = new Pen(Color.Gray, 2f);
int size = mGridData.Length;
for (int i = 0; i < size; ++i)
{
// 画横线
g.DrawLine(pen, StartX, StartY + i * GridW, StartX + size * GridW, StartY + i * GridW);
// 画竖线
g.DrawLine(pen, StartX + i * GridW, StartY, StartX + i * GridW, StartY + size * GridW);
}
// 画最后边上两条直线
g.DrawLine(pen, StartX, StartY + size * GridW, StartX + size * GridW, StartY + size * GridW);
g.DrawLine(pen, StartX + size * GridW, StartY, StartX + size * GridW, StartY + size * GridW);
// 绘制表格元素
Brush b = new SolidBrush(Color.Black);
for (int i = 0; i < mGridData.Length; i++)
{
int[] d = mGridData[i];
for (int j = 0; j < d.Length; ++j)
{
int v = d[j];
if (v > 0)
{
// 绘制阻挡信息
g.FillRectangle(b, StartX + i * GridW + 2, StartY + j * GridW + 2, GridW - 3, GridW - 3);
}
}
}
// 绘制起始点
Brush bb = new SolidBrush(Color.Green);
g.FillRectangle(bb, StartX + BeginX * GridW + 2, StartY + BeginY * GridW + 2, GridW - 3, GridW - 3);
Brush eb = new SolidBrush(Color.Red);
g.FillRectangle(eb, StartX + EndX * GridW + 2, StartY + EndY * GridW + 2, GridW - 3, GridW - 3);
}
/// <summary>
/// 【按钮点击事件】开始寻路,并绘制寻路闭合列表和最优路径
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void button2_Click(object sender, EventArgs e)
{
// 进行A* 寻路,假设相邻两个各自长度都是1
List<SearchGrid> openList = new List<SearchGrid>();
List<SearchGrid> closeList = new List<SearchGrid>();
// 1. 设置当前点为起点
SearchGrid n = new SearchGrid(this, (int)BeginX, (int)BeginY, null);
openList.Add(n);
SearchGrid cur = null;
while (true)
{
if (openList.Count == 0)
break;
// 从openlist中选取一个最近的点(F最小)作为当前节点
float minF = -1f;
SearchGrid minN = null;
foreach (var on in openList)
{
float f = on.F();
if (f < minF || minF < 0f)
{
minF = f;
minN = on;
}
}
if (minN != null)
{
cur = minN;
List<SearchGrid> nearList = GetNearNodes(cur, openList, closeList);
openList.AddRange(nearList);
openList.Remove(cur);
closeList.Add(cur);
if (cur.Same((int)EndX, (int)EndY))
{
break;
}
}
}
// 绘制
Graphics g = this.CreateGraphics();
Brush fillCloseBrush = new SolidBrush(Color.DarkGray);
Brush roadBrush = new SolidBrush(Color.Yellow);
Brush textBrush = new SolidBrush(Color.Black);
Font textFont = new Font("宋体", 12);
foreach(var cn in closeList)
{
cn.PaintGrid(g, fillCloseBrush, textBrush, textFont);
}
// 绘制最终路径
if (cur != null && cur.Same((int)EndX, (int)EndY))
{
SearchGrid nn = cur;
while (nn != null)
{
nn.PaintGrid(g, roadBrush, textBrush, textFont);
nn = nn.GetParent();
}
}
}
public List<SearchGrid> GetNearNodes(SearchGrid g, List<SearchGrid> openList = null, List<SearchGrid> closeList = null)
{
List<SearchGrid> ret = new List<SearchGrid>();
if (g != null)
{
int min = 0;
int max = mGridData.Length;
// 左边
int x1 = g.X - 1, y1 = g.Y;
CheckAddGrid(g, x1, y1, openList, closeList, ref ret);
// 右边
int x2 = g.X + 1, y2 = g.Y;
CheckAddGrid(g, x2, y2, openList, closeList, ref ret);
// 上边
int x3 = g.X, y3 = g.Y - 1;
CheckAddGrid(g, x3, y3, openList, closeList, ref ret);
// 下边
int x4 = g.X, y4 = g.Y + 1;
CheckAddGrid(g, x4, y4, openList, closeList, ref ret);
}
return ret;
}
public bool Crossable(int x, int y)
{
return mGridData[x][y] == 0;
}
public void CheckAddGrid(SearchGrid parent, int x, int y, List<SearchGrid> openList, List<SearchGrid> closeList, ref List<SearchGrid> result)
{
int min = 0;
int max = mGridData.Length;
// 1. 判断坐标是否越界和阻挡
if (x >= min && x < max && y >= min && y < max && Crossable(x, y))
{
// 2. 如果不在闭合列表,则继续
if (closeList == null || closeList.Find((e) => { return e.Same(x, y); }) == null)
{
bool found = false;
if (openList != null)
{
var opn = openList.Find((e) => { return e.Same(x, y); });
if (opn != null)
{
found = true;
float oldG = opn.G();
float newG = parent.G() + 1;
if (newG < oldG)
{
opn.SetParent(parent); // 已经在openlist中,检查是否需要更新parent
}
}
}
if (!found)
{
result.Add(new SearchGrid(this, x, y, parent));
}
}
}
}
}
}
上一篇: java简单排序算法之插入排序