8方位寻路算法
最编程
2024-06-25 10:55:46
...
鼠标左键 画障碍物
鼠标右键 寻路到鼠标所在位置
按钮 清空障碍物和界面
寻路算法类
using System;
using System.Drawing;
namespace FindPath
{
/// <summary>
/// 寻路对象
/// </summary>
public class PathFindingProvider
{
/// <summary>
/// 路径节点
/// </summary>
public class PathNode
{
/// <summary>
/// X坐标
/// </summary>
public Int32 X { get; set; }
/// <summary>
/// Y坐标
/// </summary>
public Int32 Y { get; set; }
/// <summary>
/// 下一个节点
/// </summary>
public PathNode Next { get; set; }
}
private class NodeStatus
{
/// <summary>
/// 节点的X坐标
/// </summary>
public Int32 X { get; set; }
/// <summary>
/// 节点的Y坐标
/// </summary>
public Int32 Y { get; set; }
/// <summary>
/// 节点的上级节点
/// </summary>
public Int32 Father { get; set; }
/// <summary>
/// 计数
/// </summary>
public Int32 D1 { get; set; }
/// <summary>
/// 距离
/// </summary>
public Double Distance { get; set; }
/// <summary>
/// 下一个节点 -1 为没有
/// </summary>
public Int32 NextTo { get; set; }
/// <summary>
/// 当前节点索引
/// </summary>
public Int32 Index { get; set; }
}
private class Closed_map
{
public Int32 NuDenum { get; set; }
public Int32 Mapval { get; set; }
}
/// <summary>
/// 构造寻路数据
/// </summary>
/// <param name="mapSize">地图的大小</param>
public PathFindingProvider(Size mapSize)
{
VerifyCallback = null;
Tmpe = new Closed_map[mapSize.Width, mapSize.Height];
PathLegthLimit = 5000;
Nude = new NodeStatus[PathLegthLimit];
for (int i = 0; i < PathLegthLimit; i++)
{
Nude[i] = new NodeStatus();
}
for (int x = 0; x < mapSize.Width; x++)
{
for (int y = 0; y < mapSize.Height; y++)
{
Tmpe[x, y] = new Closed_map();
}
}
}
/// <summary>
/// 验证障碍物回调,返回true可以通过,返回false不能通过
/// </summary>
public Func<Point, Boolean> VerifyCallback { get; set; }
/// <summary>
/// 限制节点长度,超过长度则寻路失败
/// </summary>
public Int32 PathLegthLimit { get; set; }
private NodeStatus[] Nude { get; set; }
private Closed_map[,] Tmpe { get; set; }
private Int32 Opened { get; set; }
private Int32 Maxnum { get; set; }
/// <summary>
/// 获取一个打开的节点
/// </summary>
/// <returns></returns>
private Int32 Getopenednude()
{
if (Opened == -1)
{
return 0;
}
else
{
var t = Opened;
Opened = Nude[t].NextTo;
return t;
}
}
/// <summary>
/// 执行寻路算法
/// </summary>
/// <param name="Start">起始坐标</param>
/// <param name="End">目标坐标</param>
/// <returns>返回第一个节点</returns>
public PathNode Execute(Point Start, Point End)
{
if (Start.X == End.X && Start.Y == End.Y)
{
return null;
}
Opened = 0;
Nude[0].X = Start.X;
Nude[0].Y = Start.Y;
Nude[0].Father = -1;
Nude[0].D1 = 0;
Nude[0].Distance = Math.Sqrt(Math.Pow((Start.X - End.X), 2) + Math.Pow((Start.Y - End.Y), 2));
Nude[0].NextTo = -1;
Nude[0].Index = 0;
Tmpe[Start.X, Start.Y].NuDenum = 0;
Tmpe[Start.X, Start.Y].Mapval = 1;
do
{
var QQ = Getopenednude();
var ISt = DetailedSearch(Nude[QQ], ref End);
var NextId = Maxnum;
if (ISt > 0)
{
if (Math.Sqrt((End.X - Nude[Maxnum].X) ^ 2 + (End.Y - Nude[Maxnum].Y) ^ 2) > 2)
{
return null;
}
var node = Nude[Maxnum];
PathNode currentNode = null;
PathNode lastNode = null;
do
{
currentNode = new PathNode()
{
X = node.X,
Y = node.Y,
Next = lastNode,
};
lastNode = currentNode;
node = Nude[node.Father];
} while (node.Father != -1);
return currentNode.Next;
}
if (Maxnum >= PathLegthLimit)
{
return null;
}
} while (Opened != -1);
return null;
}
/// <summary>
/// 继续搜索
/// </summary>
/// <param name="start"></param>
/// <param name="end"></param>
/// <returns></returns>
private Int32 DetailedSearch(NodeStatus start, ref Point end)
{
for (int i = -1; i < 2; i++)
{
for (int j = -1; j < 2; j++)
{
if (i != 0 || j != 0)
{
//结束
if (start.X + i == end.X && start.Y + j == end.Y)
{
var M = Tmpe[start.X, start.Y].NuDenum;
Maxnum++;
if (Maxnum >= PathLegthLimit)
{
return 0;
}
var X1 = start.X + i;
var Y1 = start.Y + j;
Nude[Maxnum].X = X1;
Nude[Maxnum].Y = Y1;
Nude[Maxnum].Father = M;
return Maxnum;
}
var Flags = false;
if (i * j == 0)
{
if (start.X + i >= 0 && start.Y + j >= 0)
{
Flags = !VerifyCallback(new Point(start.X + i, start.Y + j));
}
}
else
{
if (start.X + i >= 0 &&
start.Y + j >= 0 &&
start.X >= 0 &&
start.Y >= 0)
{
Flags = (!VerifyCallback(new Point(start.X + i, start.Y + j)) &&
!VerifyCallback(new Point(start.X + i, start.Y)) &&
!VerifyCallback(new Point(start.X, start.Y + j)));
}
}
if (Flags)
{
if (Tmpe[start.X + i, start.Y + j].Mapval == 1)
{
var mme = Tmpe[start.X + i, start.Y + j].NuDenum;
var aaa = Tmpe[start.X, start.Y].NuDenum;
var Fatnum = Nude[aaa].Father;
if (Fatnum != -1)
{
if (Nude[Fatnum].D1 > Nude[mme].D1)
{
Nude[aaa].Father = mme;
}
}
}
if (Tmpe[start.X + i, start.Y + j].Mapval == 0)
{
var M = Tmpe[start.X, start.Y].NuDenum;
Maxnum++;
if (Maxnum >= PathLegthLimit)
{
return 0;
}
var X1 = start.X + i;
var Y1 = start.Y + j;
Tmpe[X1, Y1].Mapval = 1;
Tmpe[X1, Y1].NuDenum = Maxnum;
Nude[Maxnum].X = X1;
Nude[Maxnum].Y = Y1;
Nude[Maxnum].Father = M;
Nude[Maxnum].D1 = Nude[M].D1 + 1;
Nude[Maxnum].Distance = Math.Sqrt(Math.Pow((X1 - end.X), 2) + Math.Pow((Y1 - end.Y), 2));
Nude[Maxnum].Index = Maxnum;
if (Nude[Maxnum].Distance == 0)
{
return Maxnum;
}
InstOPened(Nude[Maxnum]);
}
}
}
}
}
return 0;
}
private void InstOPened(NodeStatus Mnud)
{
if (Opened == -1)
{
Mnud.NextTo = -1;
Opened = Mnud.Index;
return;
}
var f = Mnud.Distance;
var temp = Opened;
var dd = 0;
do
{
if (f < Nude[temp].Distance)
{
Mnud.NextTo = temp;
if (Opened != temp)
{
Nude[dd].NextTo = Mnud.Index;
}
else
{
Opened = Mnud.Index;
}
return;
}
dd = temp;
temp = Nude[temp].NextTo;
} while (temp != -1);
Mnud.NextTo = -1;
Nude[dd].NextTo = Mnud.Index;
return;
}
}
}
测试例子
using System;
using System.Drawing;
using System.Threading;
using System.Windows.Forms;
namespace FindPath
{
public partial class Form1 : Form
{
public Form1()
{
InitializeComponent();
Data = new Boolean[1000, 1000];
}
public Boolean[,] Data { get; set; }
private void Form1_MouseMove(object sender, MouseEventArgs e)
{
if (e.Button == MouseButtons.Left)
{
var x = (Int32)Math.Floor(e.X / 10f);
var y = (Int32)Math.Floor(e.Y / 10f);
Data[x, y] = true;
using (var g = this.CreateGraphics())
{
using (var p = new SolidBrush(Color.Red))
{
g.FillRectangle(p, new Rectangle()
{
X = x * 10 + 1,
Y = y * 10 + 1,
Width = 8,
Height = 8
});
}
}
}
}
private void button1_Click(object sender, EventArgs e)
{
this.Refresh();
Data = new Boolean[1000, 1000];
}
private void Form1_MouseDown(object sender, MouseEventArgs e)
{
if (e.Button == MouseButtons.Right)
{
var x = (Int32)Math.Floor(e.X / 10f);
var y = (Int32)Math.Floor(e.Y / 10f);
PathFindingProvider findPath = new PathFindingProvider(new Size(1000, 1000));
//障碍物回调
findPath.VerifyCallback = (p) =>
{
return Data[p.X, p.Y];
};
//
Console.WriteLine($"Start:{10}{10}=>{x},{y}");
//开始寻路
var node = findPath.Execute(new Point(10, 10), new Point(x, y));
if (node != null)
{
using (var g = this.CreateGraphics())
{
using (var p = new SolidBrush(Color.Blue))
{
do
{
g.FillRectangle(p, new Rectangle()
{
X = node.X * 10 + 1,
Y = node.Y * 10 + 1,
Width = 8,
Height = 8
});
Console.WriteLine($"{node.X},{node.Y}");
node = node.Next;
Thread.Sleep(5);
} while (node != null);
}
}
}
else
{
Console.WriteLine("此路不通");
}
}
}
}
}