A*算法,(C#,Unity版本)
流程:
- 创建open和closed集合,将起始点加入open集合
- 定义启发函数 F= G+H (G:从起点到当前的路径代价,H:不考虑不通过区域,从当前节点到终点的路径代价)
- 重复下面步骤:
①.当open集合为空时,循环结束。
②.寻找open集合中,以F排序的最小的节点作为当前节点。
③.从open中移除当前节点,并把当前节点加入到closed集合中。
④.以当前节点为中心,找周围最多8个方位(九宫格除了中间)的节点(该节点可通过且不在open和closed集合中,否则不要),更新他们的G值,将他们的父节点设置为当前节点,并加入open集合中。
⑤.若当前节点为目标点,则结束。此时,从当前节点开始逐级追溯父节点,直到回溯到起点,此时回溯的各个节点即为路径。
using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
public class AStarNode:IComparable,IEquatable<AStarNode>
{
private float f, g, h;
public AStarNode parent = null;
public int x, y;
public float F => f;
public float G
{
set
{
if (Mathf.Approximately(g, value)) return;
g = value;
f = g + h;
}
get => g;
}
public float H
{
set
{
if (Mathf.Approximately(h, value)) return;
h = value;
f = g + h;
}
get => h;
}
public float Distance(AStarNode aStarNode)
{
return Mathf.Sqrt((aStarNode.x - x) * (aStarNode.x - x) + (aStarNode.y - y) * (aStarNode.y - y));
}
public AStarNode(int x,int y)
{
this.x = x;
this.y = y;
Reset();
}
public AStarNode(int x,int y,AStarNode parent)
{
Reset();
this.parent = parent;
this.x = x;
this.y = y;
}
public void Reset()
{
g = 0;
h = 0;
f = 0;
parent = null;
}
public int CompareTo(object obj)
{
AStarNode aStarNode = obj as AStarNode;
return F.CompareTo(aStarNode.F);
}
public override bool Equals(object obj)
{
if (ReferenceEquals(null, obj)) return false;
if (ReferenceEquals(this, obj)) return true;
if (obj.GetType() != this.GetType()) return false;
return Equals((AStarNode)obj);
}
public Vector2 Position => new Vector2(x, y);
public bool Equals(AStarNode other)
{
return x == other.x && y == other.y;
}
}
public struct NodeInfo
{
public int x;
public int y;
public float deltaG;
}
public class AStarManager
{
private static AStarManager instance;
public static AStarManager Instance => instance ??= new AStarManager();
private AStarNode[,] aStarNodes;
private GameObject[,] objects;
private int row = 10;
private int col = 10;
private int[,] maps;
private List<AStarNode> openSet = new List<AStarNode>();
private List<AStarNode> closedSet = new List<AStarNode>();
private AStarNode startNode;
private AStarNode endNode;
private AStarNode currentNode;
private List<AStarNode> paths = new List<AStarNode>();
public List<AStarNode> Path => paths;
public bool Find => paths is { Count: > 0 };
private readonly float sqr2 = Mathf.Sqrt(2);
/// <summary>
///
/// </summary>
/// <param name="maps">0 可行走,1表示障碍</param>
public void SetMap(int[,] maps)
{
if (objects != null)
{
for (int i = 0; i < row; i++)
{
for (int j = 0; j < col; j++)
{
UnityEngine.Object.Destroy(objects[i,j]);
}
}
objects = null;
}
paths.Clear();
this.maps = maps;
row = this.maps.GetLength(0);
col = this.maps.GetLength(1);
Debug.Log($"row:{row} col:{col}");
}
public void SetStartPosition(int x, int y)
{
startNode = new AStarNode(x, y);
Debug.Log($"start pos:{x}-{y}");
}
public void SetEndPosition(int x, int y)
{
endNode = new AStarNode(x, y);
Debug.Log($"end pos:{x}-{y}");
}
public void FindPath()
{
if(startNode == null || endNode == null)
return ;
List<AStarNode> nodes = new List<AStarNode>();
openSet.Clear();
closedSet.Clear();
openSet.Add(startNode);
while (openSet.Count>0)
{
openSet.Sort();
currentNode = openSet[0];
openSet.Remove(currentNode);
closedSet.Add(currentNode);
Queue<NodeInfo> queue = AddSurroundingNodes();
while (queue.Count>0)
{
NodeInfo nodeInfo = queue.Dequeue();
if (maps[nodeInfo.x, nodeInfo.y] == 1) continue;
AStarNode node = new AStarNode(nodeInfo.x, nodeInfo.y, currentNode);
if (openSet.Contains(node) || closedSet.Contains(node)) continue;
if (node.Equals(endNode))
{
currentNode = node;
while (currentNode.parent!=null)
{
paths.Add(currentNode);
currentNode = currentNode.parent;
}
paths.Reverse();
openSet.Clear();
queue.Clear();
Debug.Log($"找到路径,路径点个数: {paths.Count}");
return;
}
node.G = currentNode.G + nodeInfo.deltaG;
// node.G = node.Distance(startNode);
node.H = node.Distance(endNode);
openSet.Add(node);
}
}
}
private Queue<NodeInfo> AddSurroundingNodes()
{
Queue<NodeInfo> queue = new Queue<NodeInfo>();
int x = currentNode.x;
int y = currentNode.y - 1;
if (y >= 0)
{
queue.Enqueue(new NodeInfo()
{
x=x,y=y,deltaG = 1
});
}
y = currentNode.y + 1;
if (y < col)
{
queue.Enqueue(new NodeInfo()
{
x=x,y=y,deltaG = 1
});
}
x = currentNode.x - 1;
if (x >= 0)
{
y = currentNode.y - 1;
if (y >= 0)
{
queue.Enqueue(new NodeInfo()
{
x=x,y=y,deltaG = sqr2
});
}
queue.Enqueue(new NodeInfo()
{
x=x,y=currentNode.y,deltaG = 1
});
y = currentNode.y + 1;
if (y < col)
{
queue.Enqueue(new NodeInfo()
{
x=x,y=y,deltaG = sqr2
});
}
}
x = currentNode.x + 1;
if (x < row)
{
y = currentNode.y - 1;
if (y >= 0)
{
queue.Enqueue(new NodeInfo()
{
x=x,y=y,deltaG = sqr2
});
}
queue.Enqueue(new NodeInfo()
{
x=x,y=currentNode.y,deltaG = 1
});
y = currentNode.y + 1;
if (y < col)
{
queue.Enqueue(new NodeInfo()
{
x=x,y=y,deltaG = sqr2
});
}
}
return queue;
}
public IEnumerator StepShowPath()
{
if(startNode == null || endNode == null)
yield return null;
ShowMap();
List<AStarNode> nodes = new List<AStarNode>();
openSet.Clear();
closedSet.Clear();
openSet.Add(startNode);
while (openSet.Count>0)
{
openSet.Sort();
currentNode = openSet[0];
objects[currentNode.x, currentNode.y].GetComponent<MeshRenderer>().material.color = Color.blue;
yield return new WaitForSeconds(1f);
openSet.Remove(currentNode);
closedSet.Add(currentNode);
Queue<NodeInfo> queue = AddSurroundingNodes();
while (queue.Count>0)
{
NodeInfo nodeInfo = queue.Dequeue();
if (maps[nodeInfo.x, nodeInfo.y] == 1) continue;
AStarNode node = new AStarNode(nodeInfo.x, nodeInfo.y, currentNode);
if (openSet.Contains(node) || closedSet.Contains(node)) continue;
if (node.Equals(endNode))
{
currentNode = node;
while (currentNode.parent!=null)
{
paths.Add(currentNode);
currentNode = currentNode.parent;
}
paths.Reverse();
openSet.Clear();
queue.Clear();
Debug.Log($"找到路径,路径点个数: {paths.Count}");
ShowPath();
yield break;
}
node.G = currentNode.G + nodeInfo.deltaG;
// node.G = node.Distance(startNode);
node.H = node.Distance(endNode);
openSet.Add(node);
objects[node.x, node.y].GetComponent<MeshRenderer>().material.color = Color.cyan;
yield return new WaitForSeconds(0.5f);
}
}
}
public void ShowMap()
{
if (objects == null)
{
objects = new GameObject[row, col];
}
for (int i = 0; i < row; i++)
{
for (int j = 0; j < col; j++)
{
GameObject obj = GameObject.CreatePrimitive(PrimitiveType.Cube);
objects[i, j] = obj;
obj.name = $"{i}-{j}";
obj.GetComponent<MeshRenderer>().material = new Material(Shader.Find("Unlit/SimpleColor"));
obj.GetComponent<MeshRenderer>().material.color = maps[i, j] == 0 ? Color.white : Color.red;
obj.transform.position = new Vector3(j,i,0);
}
}
if (startNode != null)
{
objects[startNode.x,startNode.y].GetComponent<MeshRenderer>().material.color = Color.magenta;
}
if (endNode != null)
{
objects[endNode.x,endNode.y].GetComponent<MeshRenderer>().material.color = Color.yellow;
}
}
public void ShowPath()
{
if(objects == null || paths == null)return;
foreach (var item in paths)
{
objects[item.x,item.y].GetComponent<MeshRenderer>().material.color = Color.green;
}
if (startNode != null)
{
objects[startNode.x,startNode.y].GetComponent<MeshRenderer>().material.color = Color.magenta;
}
if (endNode != null)
{
objects[endNode.x,endNode.y].GetComponent<MeshRenderer>().material.color = Color.yellow;
}
}
}
using UnityEngine;
public class AStarTest : MonoBehaviour
{
// Start is called before the first frame update
void Start()
{
AStarManager.Instance.SetMap(new int[,]
{
{1,1,1,1,1,1,1,1,1,1},
{1,0,0,0,0,0,0,0,0,1},
{1,0,0,0,1,0,0,0,1,1},
{1,0,0,0,1,0,1,0,1,1},
{1,0,0,0,1,0,1,0,0,1},
{1,0,0,0,0,0,1,1,0,1},
{1,0,0,0,0,0,0,1,0,1},
{1,0,0,0,0,0,0,0,0,1},
{1,1,1,1,1,1,1,1,1,1},
});
AStarManager.Instance.SetStartPosition(1,1);
AStarManager.Instance.SetEndPosition(7,7);
// AStarManager.Instance.SetEndPosition(4,7);
// AStarManager.Instance.FindPath();
// AStarManager.Instance.ShowMap();
// AStarManager.Instance.ShowPath();
StartCoroutine(AStarManager.Instance.StepShowPath());
}
}