//寻路评估函数
private float Heuristic(CStarNode node)
{
int dcol = m_endNode.Col - node.Col;
int drow = m_endNode.Row - node.Row;
return(Math.Abs(dcol) * m_straightCost + Math.Abs(drow) * m_straightCost);
}
/// <summary>
/// 填充本node的值
/// </summary>
/// <param name="f">F.</param>
/// <param name="g">G.</param>
/// <param name="h">H.</param>
/// <param name="parent">Parent.</param>
public void FillValue(float f, float g, float h, CStarNode parent)
{
F = f;
G = g;
H = h;
Parent = parent;
}
/// <summary>
/// 数据来源AStar 表格地图的直线寻路结果
/// </summary>
/// <returns><c>true</c>, if path was found, <c>false</c> otherwise.</returns>
/// <param name="grid">Grid.</param>
public bool FindPath(CStarGrid grid)
{
CStarNode startNode = grid.StartNode;
CStarNode endNode = grid.EndNode;
SetData(startNode.Col, startNode.Row, endNode.Col, endNode.Row);
return(Find(grid));
}
/// <summary>
/// Finds the path.
/// result do not has the start node itself
/// </summary>
/// <returns><c>true</c>, if path was found, <c>false</c> otherwise.</returns>
/// <param name="grid">传入需要进行寻路的地图</param>
/// <param name="closeToDestination">If set to <c>false</c>, 寻路结果不包含终点</param>
public bool FindPath(CStarGrid grid, bool closeToDestination = false)
{
m_grid = grid;
m_findClosestNode = closeToDestination;
m_open.Clear();
m_closed.Clear();
m_startNode = m_grid.StartNode;
m_endNode = m_grid.EndNode;
//终点不可通行, 我就不寻路了
if (!m_endNode.Walkable)
{
return(false);
}
m_startNode.G = 0;
m_startNode.H = Heuristic(m_startNode);
m_startNode.F = m_startNode.G + m_startNode.H;
bool result = Search();
return(result);
}
private void BuildPath()
{
m_path.Clear();
CStarNode node = m_endNode;
if (!m_findClosestNode)
{
m_path.Push(node);
}
while (node != m_startNode)
{
node = node.Parent;
m_path.Push(node);
}
/* remove the first node which indeed is start node */
if (m_path.Count > 0)
{
m_path.Pop();
}
}
//寻路的具体算法
private bool Search()
{
CStarNode node = m_startNode;
while (node != m_endNode)
{
int startCol = Math.Max(0, node.Col - 1);
int endCol = Math.Min(m_grid.NumCols - 1, node.Col + 1);
int startRow = Math.Max(0, node.Row - 1);
int endRow = Math.Min(m_grid.NumRows - 1, node.Row + 1);
//默认我们是寻找九宫格的可通行性
//但对于2d游戏, 我们想仅仅寻找左右上下
for (int i = startRow; i <= endRow; i++)
{
for (int j = startCol; j <= endCol; j++)
{
CStarNode test = m_grid.GetNode(j, i);
if (test == node)
{
continue;
}
if (!test.Walkable)
{
continue;
}
//这里之前是在后面也做f更新了. 但考虑提前到这里会提高效率
//如果寻路结果错误, 要移回去
if (IsClosed(test))
{
continue;
}
//如果仅计算上下左右, 就4个斜角的就不计算了
if (m_conn == Connection.Four && node.Col != test.Col && node.Row != test.Row)
{
continue;
}
//设置相邻两个格子的通行代价
float cost = m_diagCost;
if (node.Col == test.Col || node.Row == test.Row)
{
cost = m_straightCost;
}
float g = node.G + cost * test.CostMultiplier;
float h = Heuristic(test);
float f = g + h;
if (IsOpen(test))
{
if (test.F > f)
{
test.FillValue(f, g, h, node);
}
}
else
{
test.FillValue(f, g, h, node);
m_open.Push(test);
}
}
}
m_closed.Add(node);
if (m_open.Count == 0)
{
return(false);
}
node = m_open.Pop();
}
BuildPath();
return(true);
}
private bool IsClosed(CStarNode node)
{
return(m_closed.Contains(node));
}
private bool IsOpen(CStarNode node)
{
return(m_open.Contains(node));
}