U3d中的A*算法调试

最新推荐文章于 2024-01-07 21:58:49 发布

aicuo0004

最新推荐文章于 2024-01-07 21:58:49 发布

阅读量657

点赞数

using UnityEngine;

using System.Collections;
using System.Collections.Generic;

/// <summary>
/// AStarNode 用于保存规划到当前节点时的各个Cost值以及父节点。
/// zhuweisky 2008.10.18
/// </summary>
public class AStarNode
{
#region Ctor
public AStarNode(Point loc, AStarNode previous, int _costG, int _costH)
{
this.location = loc;
this.previousNode = previous;
this.costG = _costG;
this.costH = _costH;
}
#endregion

#region Location
private Point location = new Point(0, 0);
/// <summary>
/// Location 节点所在的位置，其X值代表ColumnIndex，Y值代表LineIndex
/// </summary>
public Point Location
{
get { return location; }
}
#endregion

#region PreviousNode
private AStarNode previousNode = null;
/// <summary>
/// PreviousNode 父节点，即是由哪个节点导航到当前节点的。
/// </summary>
public AStarNode PreviousNode
{
get { return previousNode; }
}
#endregion

#region CostF
/// <summary>
/// CostF 从起点导航经过本节点然后再到目的节点的估算总代价。
/// </summary>
public int CostF
{
get
{
return this.costG + this.costH;
}
}
#endregion

#region CostG
private int costG = 0;
/// <summary>
/// CostG 从起点导航到本节点的代价。
/// </summary>
public int CostG
{
get { return costG; }
}
#endregion

#region CostH
private int costH = 0;
/// <summary>
/// CostH 使用启发式方法估算的从本节点到目的节点的代价。
/// </summary>
public int CostH
{
get { return costH; }
}
#endregion

#region ResetPreviousNode
/// <summary>
/// ResetPreviousNode 当从起点到达本节点有更优的路径时，调用该方法采用更优的路径。
/// </summary>
public void ResetPreviousNode(AStarNode previous, int _costG)
{
this.previousNode = previous;
this.costG = _costG;
}
#endregion

public override string ToString()
{
return this.location.ToString();
}
}

public enum CompassDirections
{
NotSet = 0,
North = 1, //UP
NorthEast = 2, //UP Right
East = 3,
SouthEast = 4,
South = 5,
SouthWest = 6,
West = 7,
NorthWest = 8
}

/// <summary>
/// ICostGetter 获取从当前节点向某个方向移动时的代价。
/// </summary>
public interface ICostGetter
{
int GetCost(Point currentNodeLoaction, CompassDirections moveDirection);
}

/// <summary>
/// SimpleCostGetter ICostGetter接口的简化实现。直线代价为10，斜线为14。
/// </summary>
public class SimpleCostGetter : ICostGetter
{
#region ICostGetter 成员

public int GetCost(Point currentNodeLoaction, CompassDirections moveDirection)
{
if (moveDirection == CompassDirections.NotSet)
{
return 0;
}

if (moveDirection == CompassDirections.East || moveDirection == CompassDirections.West || moveDirection == CompassDirections.South || moveDirection == CompassDirections.North)
{
return 10;
}

return 14;
}

#endregion
}

/// <summary>
/// RoutePlanData 用于封装一次路径规划过程中的规划信息。
/// </summary>
public class RoutePlanData
{
#region CellMap
private Rectangle cellMap;
/// <summary>
/// CellMap 地图的矩形大小。经过单元格标准处理。
/// </summary>
public Rectangle CellMap
{
get { return cellMap; }
}
#endregion

#region ClosedList
private List<AStarNode> closedList = new List<AStarNode>();
/// <summary>
/// ClosedList 关闭列表，即存放已经遍历处理过的节点。
/// </summary>
public List<AStarNode> ClosedList
{
get { return closedList; }
}
#endregion

#region OpenedList
private List<AStarNode> openedList = new List<AStarNode>();
/// <summary>
/// OpenedList 开放列表，即存放已经开发但是还未处理的节点。
/// </summary>
public List<AStarNode> OpenedList
{
get { return openedList; }
}
#endregion

#region Destination
private Point destination;
/// <summary>
/// Destination 目的节点的位置。
/// </summary>
public Point Destination
{
get { return destination; }
}
#endregion

#region Ctor
public RoutePlanData(Rectangle map, Point _destination)
{
this.cellMap = map;
this.destination = _destination;
}
#endregion
}

/// <summary>
/// AStarRoutePlanner A*路径规划。每个单元格Cell的位置用Point表示
/// F = G + H 。
/// G = 从起点A，沿着产生的路径，移动到网格上指定方格的移动耗费。
/// H = 从网格上那个方格移动到终点B的预估移动耗费。使用曼哈顿方法，它计算从当前格到目的格之间水平和垂直的方格的数量总和，忽略对角线方向。
/// zhuweisky 2008.10.18
/// </summary>
public class AStarRoutePlanner
{
private int lineCount = 10; //反映地图高度，对应Y坐标
private int columnCount = 10; //反映地图宽度，对应X坐标
private ICostGetter costGetter = new SimpleCostGetter();
private bool[][] obstacles = null; //障碍物位置，维度：Column * Line

#region Ctor
public AStarRoutePlanner() :this(10 ,10 ,new SimpleCostGetter())
{
}
public AStarRoutePlanner(int _lineCount, int _columnCount, ICostGetter _costGetter)
{
this.lineCount = _lineCount;
this.columnCount = _columnCount;
this.costGetter = _costGetter;

this.InitializeObstacles();
}

/// <summary>
/// InitializeObstacles 将所有位置均标记为无障碍物。
/// </summary>
private void InitializeObstacles()
{
this.obstacles = new bool[this.columnCount][];
for (int i = 0; i < this.columnCount; i++)
{
this.obstacles[i] = new bool[this.lineCount];
}

for (int i = 0; i < this.columnCount; i++)
{
for (int j = 0; j < this.lineCount; j++)
{
this.obstacles[i][j] = false;
}
}
}
#endregion

#region Initialize
/// <summary>
/// Initialize 在路径规划之前先设置障碍物位置。
/// </summary>
public void Initialize(List<Point> obstaclePoints)
{
if (obstacles != null)
{
foreach (Point pt in obstaclePoints)
{
this.obstacles[pt.X][pt.Y] = true;
}
}
}
#endregion

#region Plan
public List<Point> Plan(Point start, Point destination)
{
Rectangle map = new Rectangle(0, 0, this.columnCount, this.lineCount);
if ((!map.Contains(start)) || (!map.Contains(destination)))
{
//throw new Exception("StartPoint or Destination not in the current map!");
Debug.LogError("StartPoint or Destination not in the current map!");
}

RoutePlanData routePlanData = new RoutePlanData(map, destination);

AStarNode startNode = new AStarNode(start, null, 0, 0);
routePlanData.OpenedList.Add(startNode);

AStarNode currenNode = startNode;

//从起始节点开始进行递归调用
return DoPlan(routePlanData, currenNode);
}
#endregion

#region DoPlan
private List<Point> DoPlan(RoutePlanData routePlanData, AStarNode currenNode)
{
List<CompassDirections> allCompassDirections = CompassDirectionsHelper.GetAllCompassDirections();
foreach (CompassDirections direction in allCompassDirections)
{
Point nextCell = GeometryHelper.GetAdjacentPoint(currenNode.Location, direction);
if (!routePlanData.CellMap.Contains(nextCell)) //相邻点已经在地图之外
{
continue;
}

if (this.obstacles[nextCell.X][nextCell.Y]) //下一个Cell为障碍物
{
continue;
}

int costG = this.costGetter.GetCost(currenNode.Location, direction);
int costH = Mathf.Abs(nextCell.X - routePlanData.Destination.X) + Mathf.Abs(nextCell.Y - routePlanData.Destination.Y);
if (costH == 0) //costH为0，表示相邻点就是目的点，规划完成，构造结果路径
{
List<Point> route = new List<Point>();
route.Add(routePlanData.Destination);
route.Insert(0, currenNode.Location);
AStarNode tempNode = currenNode;
while (tempNode.PreviousNode != null)
{
route.Insert(0, tempNode.PreviousNode.Location);
tempNode = tempNode.PreviousNode;
}

return route;
}

AStarNode existNode = this.GetNodeOnLocation(nextCell, routePlanData);
if (existNode != null)
{
if (existNode.CostG > costG)
{
//如果新的路径代价更小，则更新该位置上的节点的原始路径
existNode.ResetPreviousNode(currenNode, costG);
}
}
else
{
AStarNode newNode = new AStarNode(nextCell, currenNode, costG, costH);
routePlanData.OpenedList.Add(newNode);
}
}

//将已遍历过的节点从开放列表转移到关闭列表
routePlanData.OpenedList.Remove(currenNode);
routePlanData.ClosedList.Add(currenNode);

AStarNode minCostNode = this.GetMinCostNode(routePlanData.OpenedList);
if (minCostNode == null) //表明从起点到终点之间没有任何通路。
{
return null;
}

//对开放列表中的下一个代价最小的节点作递归调用
return this.DoPlan(routePlanData, minCostNode);
}
#endregion

#region GetNodeOnLocation
/// <summary>
/// GetNodeOnLocation 目标位置location是否已存在于开放列表或关闭列表中
/// </summary>
private AStarNode GetNodeOnLocation(Point location, RoutePlanData routePlanData)
{
foreach (AStarNode temp in routePlanData.OpenedList)
{
if (temp.Location == location)
{
return temp;
}
}

foreach (AStarNode temp in routePlanData.ClosedList)
{
if (temp.Location == location)
{
return temp;
}
}

return null;
}
#endregion

#region GetMinCostNode
/// <summary>
/// GetMinCostNode 从开放列表中获取代价F最小的节点，以启动下一次递归
/// </summary>
private AStarNode GetMinCostNode(List<AStarNode> openedList)
{
if (openedList.Count == 0)
{
return null;
}

AStarNode target = openedList[0];
foreach (AStarNode temp in openedList)
{
if (temp.CostF < target.CostF)
{
target = temp;
}
}

return target;
}
#endregion
}

public class Rectangle
{
int startColumn;
int startLine;
int columnCount;
int lineCount;
List<Point> map;
public Rectangle(int startColumn,int startLine,int columnCount,int lineCount)
{
this.startColumn = startColumn;
this.startLine = startLine;
this.columnCount = columnCount;
this.lineCount = lineCount;

map = new List<Point>();
for (int i = startColumn; i < columnCount; i++) {
for (int j = startLine; j < lineCount; j++) {
Point point = new Point(i,j);
map.Add(point);
}
}
}

public bool Contains(Point point)
{
foreach (Point item in map) {
if (point.X == item.X && point.Y == item.Y) {
return true;
}
}

return false;
}
}

public static class CompassDirectionsHelper
{
private static List<CompassDirections> AllCompassDirections = new List<CompassDirections>();

#region Static Ctor
static CompassDirectionsHelper()
{
CompassDirectionsHelper.AllCompassDirections.Add(CompassDirections.East);
CompassDirectionsHelper.AllCompassDirections.Add(CompassDirections.West);
CompassDirectionsHelper.AllCompassDirections.Add(CompassDirections.South);
CompassDirectionsHelper.AllCompassDirections.Add(CompassDirections.North);

CompassDirectionsHelper.AllCompassDirections.Add(CompassDirections.SouthEast);
CompassDirectionsHelper.AllCompassDirections.Add(CompassDirections.SouthWest);
CompassDirectionsHelper.AllCompassDirections.Add(CompassDirections.NorthEast);
CompassDirectionsHelper.AllCompassDirections.Add(CompassDirections.NorthWest);
}
#endregion

#region GetAllCompassDirections
public static List<CompassDirections> GetAllCompassDirections()
{
return CompassDirectionsHelper.AllCompassDirections;
}
#endregion
}

public class Point
{
private int x;

public Point(int x,int y)
{
this.x = x;
this.y = y;
}

public int X
{
get{return x;}
}

private int y;

public int Y
{
get{return y;}
}
}

public static class GeometryHelper
{
#region GetAdjacentPoint
/// <summary>
/// GetAdjacentPoint 获取某个方向上的相邻点
/// </summary>
public static Point GetAdjacentPoint(Point current, CompassDirections direction)
{
switch (direction)
{
case CompassDirections.North:
{
return new Point(current.X, current.Y - 1);
}
case CompassDirections.South:
{
return new Point(current.X, current.Y + 1);
}
case CompassDirections.East:
{
return new Point(current.X + 1, current.Y);
}
case CompassDirections.West:
{
return new Point(current.X - 1, current.Y);
}
case CompassDirections.NorthEast:
{
return new Point(current.X + 1, current.Y - 1);
}
case CompassDirections.NorthWest:
{
return new Point(current.X - 1, current.Y - 1);
}
case CompassDirections.SouthEast:
{
return new Point(current.X + 1, current.Y + 1);
}
case CompassDirections.SouthWest:
{
return new Point(current.X - 1, current.Y + 1);
}
default:
{
return current;
}
}
}
#endregion

}

2.RouteTest脚本

using UnityEngine;
using System.Collections;
using System.Collections.Generic;

public class RouteTest : MonoBehaviour {

// Use this for initialization
void Start () {
AStarRoutePlanner aStarRoutePlanner = new AStarRoutePlanner();
List<Point> obstaclePoints = new List<Point>();
obstaclePoints.Add(new Point(1, 1));
obstaclePoints.Add(new Point(3, 4));
obstaclePoints.Add(new Point(4, 4));
obstaclePoints.Add(new Point(5, 4));
obstaclePoints.Add(new Point(6, 4));
aStarRoutePlanner.Initialize(obstaclePoints);

List<Point> route = aStarRoutePlanner.Plan(new Point(0, 0), new Point(7, 7));
foreach (Point item in route) {
Debug.LogError("x:" + item.X + "\t" + "y:" + item.Y);
}

}

// Update is called once per frame
void Update () {

}
}